diff --git a/Changelog.md b/CHANGELOG.md similarity index 66% rename from Changelog.md rename to CHANGELOG.md index 328027509c3a5..c2cb1c017129c 100644 --- a/Changelog.md +++ b/CHANGELOG.md @@ -1,9 +1,108 @@ ## DART 6 -### Version 6.0.0 (2015-12-19) +### DART 6.0.0 (2016-05-10) + +* Common data structures + + * Added `Node`, `Aspect`, `State`, and `Properties`: [#713](https://github.com/dartsim/dart/pull/713), [#712](https://github.com/dartsim/dart/issues/712), [#708](https://github.com/dartsim/dart/pull/708), [#707](https://github.com/dartsim/dart/pull/707), [#659](https://github.com/dartsim/dart/pull/659), [#649](https://github.com/dartsim/dart/pull/649), [#645](https://github.com/dartsim/dart/issues/645), [#607](https://github.com/dartsim/dart/pull/607), [#598](https://github.com/dartsim/dart/pull/598), [#591](https://github.com/dartsim/dart/pull/591), [#531](https://github.com/dartsim/dart/pull/531) + * Added mathematical constants and user-defined literals for radian, degree, and pi: [#669](https://github.com/dartsim/dart/pull/669), [#314](https://github.com/dartsim/dart/issues/314) + * Added `ShapeFrame` and `ShapeNode`: [#608](https://github.com/dartsim/dart/pull/608) + * Added `BoundingBox`: [#547](https://github.com/dartsim/dart/pull/547), [#546](https://github.com/dartsim/dart/issues/546) + +* Kinematics features: + + * Added convenient functions for setting joint limits: [#703](https://github.com/dartsim/dart/pull/703) + * Added more description on `InverseKinematics::solve()`: [#624](https://github.com/dartsim/dart/pull/624) + * Added API for utilizing analytical inverse kinematics: [#530](https://github.com/dartsim/dart/pull/530), [#463](https://github.com/dartsim/dart/issues/463) + * Added color property to `Marker`: [#187](https://github.com/dartsim/dart/issues/187) + * Improved `Skeleton` to clone `State` as well: [#691](https://github.com/dartsim/dart/pull/691) + * Improved `ReferentialSkeleton` to be able to add and remove `BodyNode`s and `DegreeOfFreedom`s to/from `Group`s freely: [#557](https://github.com/dartsim/dart/pull/557), [#556](https://github.com/dartsim/dart/issues/556), [#548](https://github.com/dartsim/dart/issues/548) + * Changed `Marker` into `Node`: [#692](https://github.com/dartsim/dart/pull/692), [#609](https://github.com/dartsim/dart/issues/609) + * Renamed `Joint::get/setLocal[~]` to `Joint::get/setRelative[~]`: [#715](https://github.com/dartsim/dart/pull/715), [#714](https://github.com/dartsim/dart/issues/714) + * Renamed `PositionLimited` to `PositionLimitEnforced`: [#447](https://github.com/dartsim/dart/issues/447) + * Fixed initialization of joint position and velocity: [#691](https://github.com/dartsim/dart/pull/691), [#621](https://github.com/dartsim/dart/pull/621) + * Fixed `InverseKinematics` when it's used with `FreeJoint` and `BallJoint`: [#683](https://github.com/dartsim/dart/pull/683) + * Fixed ambiguous overload on `MetaSkeleton::getLinearJacobianDeriv`: [#628](https://github.com/dartsim/dart/pull/628), [#626](https://github.com/dartsim/dart/issues/626) + +* Dynamics features: + + * Added `get/setLCPSolver` functions to `ConstraintSolver`: [#633](https://github.com/dartsim/dart/pull/633) + * Added `ServoMotorConstraint` as a preliminary implementation for `SERVO` actuator type: [#566](https://github.com/dartsim/dart/pull/566) + * Improved `ConstraintSolver` to obey C++11 ownership conventions: [#616](https://github.com/dartsim/dart/pull/616) + * Fixed segfualting of `DantzigLCPSolver` when the constraint dimension is zero: [#634](https://github.com/dartsim/dart/pull/634) + * Fixed missing implementations in ConstrainedGroup: [#586](https://github.com/dartsim/dart/pull/586) + * Fixed incorrect applying of joint constraint impulses: [#317](https://github.com/dartsim/dart/issues/317) + * Deprecated `draw()` functions of dynamics classes: [#654](https://github.com/dartsim/dart/pull/654) + +* Collision detection features + + * Added `CollisionGroup` and refactored `CollisionDetector` to be more versatile: [#711](https://github.com/dartsim/dart/pull/711), [#704](https://github.com/dartsim/dart/pull/704), [#689](https://github.com/dartsim/dart/pull/689), [#631](https://github.com/dartsim/dart/pull/631), [#642](https://github.com/dartsim/dart/issues/642), [#20](https://github.com/dartsim/dart/issues/20) + * Improved API for self collision checking options: [#718](https://github.com/dartsim/dart/pull/718), [#702](https://github.com/dartsim/dart/issues/702) + * Deprecated `BodyNode::isColliding`; collision sets are moved to `CollisionResult`: [#694](https://github.com/dartsim/dart/pull/694), [#670](https://github.com/dartsim/dart/pull/670), [#668](https://github.com/dartsim/dart/pull/668), [#666](https://github.com/dartsim/dart/issues/666) + +* Parsers + + * Added back VSK parser: [#602](https://github.com/dartsim/dart/pull/602), [#561](https://github.com/dartsim/dart/pull/561), [#254](https://github.com/dartsim/dart/issues/254) + * Fixed segfault of `SdfParser` when `nullptr` `ResourceRetriever` is passed: [#663](https://github.com/dartsim/dart/pull/663) + +* GUI features + + * Merged `renderer` namespace into `gui` namespace: [#652](https://github.com/dartsim/dart/pull/652), [#589](https://github.com/dartsim/dart/issues/589) + * Moved `osgDart` under `dart::gui` namespace as `dart::gui::osg`: [#651](https://github.com/dartsim/dart/pull/651) + * Fixed GlutWindow::screenshot(): [#623](https://github.com/dartsim/dart/pull/623), [#395](https://github.com/dartsim/dart/issues/395) + +* Simulation features + + * Fixed `World::clone()` didn't clone the collision detector: [#658](https://github.com/dartsim/dart/pull/658) + * Fixed bug of `World` concurrency: [#577](https://github.com/dartsim/dart/pull/577), [#576](https://github.com/dartsim/dart/issues/576) + +* Misc improvements and bug fixes + + * Added `make_unique` that was omitted from C++11: [#639](https://github.com/dartsim/dart/pull/639) + * Added missing `override` keywords: [#617](https://github.com/dartsim/dart/pull/617), [#535](https://github.com/dartsim/dart/pull/535) + * Added gcc warning flag `-Wextra`: [#600](https://github.com/dartsim/dart/pull/600) + * Improved memory management of `constraint` namespace: [#584](https://github.com/dartsim/dart/pull/584), [#583](https://github.com/dartsim/dart/issues/583) + * Changed the extension of headers from `.h` to `.hpp`: [#709](https://github.com/dartsim/dart/pull/709), [#693](https://github.com/dartsim/dart/pull/693), [#568](https://github.com/dartsim/dart/issues/568) + * Changed Doxyfile to gnerate tag file: [#690](https://github.com/dartsim/dart/pull/690) + * Changed the convention to use `std::size_t` over `size_t`: [#681](https://github.com/dartsim/dart/pull/681), [#656](https://github.com/dartsim/dart/issues/656) + * Changed CMake to configure preprocessors using `#cmakedefine`: [#648](https://github.com/dartsim/dart/pull/648), [#641](https://github.com/dartsim/dart/pull/641) + * Updated copyright years: [#679](https://github.com/dartsim/dart/pull/679), [#160](https://github.com/dartsim/dart/issues/160) + * Renamed directory name `apps` to `examples`: [#685](https://github.com/dartsim/dart/pull/685) + * Fixed warnings of unused variables in release mode: [#646](https://github.com/dartsim/dart/pull/646) + * Fixed typo of `getNumPluralAddoName` in utility macro: [#615](https://github.com/dartsim/dart/issues/615) + * Fixed linker error by adding namespace-scope definitions for `constexpr static` members: [#603](https://github.com/dartsim/dart/pull/603) + * Fixed segfault from nullptr meshes: [#585](https://github.com/dartsim/dart/pull/585) + * Fixed typo of tutorial with minor improvements: [#573](https://github.com/dartsim/dart/pull/573) + * Fixed `NameManager::removeEntries(~)` called a function that does not exist: [#564](https://github.com/dartsim/dart/pull/564), [#554](https://github.com/dartsim/dart/issues/554) + * Fixed missing definitions for various functions: [#558](https://github.com/dartsim/dart/pull/558), [#555](https://github.com/dartsim/dart/issues/555) + * Fixed const correctness of `BodyNode::getMomentsOfInertia()`: [#541](https://github.com/dartsim/dart/pull/541), [#540](https://github.com/dartsim/dart/issues/540) + * Fixed `ftel` bug in Linux with an workaround: [#533](https://github.com/dartsim/dart/pull/533) + * Removed unnecessary `virtual` keyword for overriding functions: [#680](https://github.com/dartsim/dart/pull/680) + * Removed deprecated APIs in DART 5: [#678](https://github.com/dartsim/dart/pull/678) + +* Build and test issues + + * Added CMake target for code coverage testing, and automatic reporting: [#688](https://github.com/dartsim/dart/pull/688), [#687](https://github.com/dartsim/dart/issues/687), [#638](https://github.com/dartsim/dart/pull/638), [#632](https://github.com/dartsim/dart/pull/632) + * Added missing `liburdfdom-dev` dependency in Ubuntu package: [#574](https://github.com/dartsim/dart/pull/574) + * Modulized DART libraries: [#706](https://github.com/dartsim/dart/pull/706), [#675](https://github.com/dartsim/dart/pull/675), [#652](https://github.com/dartsim/dart/pull/652), [#477](https://github.com/dartsim/dart/issues/477) + * Improved Travis-CI script: [#655](https://github.com/dartsim/dart/pull/655) + * Improved CMake script by splitting tutorials, examples, and tests into separate targets: [#644](https://github.com/dartsim/dart/pull/644) + * Improved wording of the cmake warning messages for ASSIMP: [#553](https://github.com/dartsim/dart/pull/553) + * Changed Travis-CI to treat warning as errors using `-Werror` flags: [#682](https://github.com/dartsim/dart/pull/682), [#677](https://github.com/dartsim/dart/issues/677) + * Changed Travis-CI to test DART with bullet collision detector: [#650](https://github.com/dartsim/dart/pull/650), [#376](https://github.com/dartsim/dart/issues/376) + * Changed the minimum requirement of Visual Studio version to 2015: [#592](https://github.com/dartsim/dart/issues/592) + * Changed CMake to build gui::osg examples when `DART_BUILD_EXAMPLES` is on: [#536](https://github.com/dartsim/dart/pull/536) + * Simplfied Travis-CI tests for general pushes: [#700](https://github.com/dartsim/dart/pull/700) + * Fixed Eigen memory alignment issue in testCollision.cpp: [#719](https://github.com/dartsim/dart/pull/719) + * Fixed `BULLET_INCLUDE_DIRS` in `DARTConfig.cmake`: [#697](https://github.com/dartsim/dart/pull/697) + * Fixed linking with Bullet on OS X El Capitan by supporting for Bullet built with double precision: [#660](https://github.com/dartsim/dart/pull/660), [#657](https://github.com/dartsim/dart/issues/657) + * Fixed FCL version check logic in the main `CMakeLists.txt`: [#640](https://github.com/dartsim/dart/pull/640) + * Fixed `find_package(DART)` on optimizer components: [#637](https://github.com/dartsim/dart/pull/637) + * Fixed linking against `${DART_LIBRARIES}` not working in Ubuntu 14.04: [#630](https://github.com/dartsim/dart/pull/630), [#629](https://github.com/dartsim/dart/issues/629) + * Fixed Visual Studio 2015 build errors: [#580](https://github.com/dartsim/dart/pull/580) + * Removed OpenGL dependency from `dart` library: [#667](https://github.com/dartsim/dart/pull/667) + * Removed version check for Bullet: [#636](https://github.com/dartsim/dart/pull/636), [#625](https://github.com/dartsim/dart/issues/625) -1. Added missing `liburdfdom-dev` dependency in Ubuntu package - * [Pull request #574](https://github.com/dartsim/dart/pull/574) ## DART 5 diff --git a/dart/collision/CollisionDetector.hpp b/dart/collision/CollisionDetector.hpp index 1f37a2c69611e..21f5ac2354103 100644 --- a/dart/collision/CollisionDetector.hpp +++ b/dart/collision/CollisionDetector.hpp @@ -98,15 +98,22 @@ class CollisionDetector : public std::enable_shared_from_this std::shared_ptr createCollisionGroupAsSharedPtr( const Args&... args); - /// Perform collision detection for group. + /// Perform collision check for a single group. If nullptr is passed to + /// result, then the this returns only simple information whether there is a + /// collision of not. virtual bool collide( CollisionGroup* group, - const CollisionOption& option, CollisionResult& result) = 0; + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) = 0; - /// Perform collision detection for group1-group2. + /// Perform collision check for two groups. If nullptr is passed to + /// result, then the this returns only simple information whether there is a + /// collision of not. virtual bool collide( - CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) = 0; + CollisionGroup* group1, + CollisionGroup* group2, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) = 0; protected: diff --git a/dart/collision/CollisionGroup.cpp b/dart/collision/CollisionGroup.cpp index bc34fbc0624d9..75ba402220639 100644 --- a/dart/collision/CollisionGroup.cpp +++ b/dart/collision/CollisionGroup.cpp @@ -155,14 +155,28 @@ std::size_t CollisionGroup::getNumShapeFrames() const } //============================================================================== -bool CollisionGroup::collide(const CollisionOption& option, CollisionResult& result) +const dynamics::ShapeFrame* CollisionGroup::getShapeFrame( + std::size_t index) const +{ + assert(index < mShapeFrameMap.size()); + if(index < mShapeFrameMap.size()) + return mShapeFrameMap[index].first; + + return nullptr; +} + +//============================================================================== +bool CollisionGroup::collide( + const CollisionOption& option, CollisionResult* result) { return mCollisionDetector->collide(this, option, result); } //============================================================================== bool CollisionGroup::collide( - CollisionGroup* other, const CollisionOption& option, CollisionResult& result) + CollisionGroup* other, + const CollisionOption& option, + CollisionResult* result) { return mCollisionDetector->collide(this, other, option, result); } diff --git a/dart/collision/CollisionGroup.hpp b/dart/collision/CollisionGroup.hpp index 9df47cc85cccb..979539772a6e6 100644 --- a/dart/collision/CollisionGroup.hpp +++ b/dart/collision/CollisionGroup.hpp @@ -105,7 +105,7 @@ class CollisionGroup /// Add ShapeFrames of Skeleton, and also add another ShapeFrames of other /// various objects. template - void addShapeFramesOf(const dynamics::Skeleton* skeleton, + void addShapeFramesOf(const dynamics::MetaSkeleton* skeleton, const Others*... others); /// Do nothing. This function is for terminating the recursive variadic @@ -168,15 +168,22 @@ class CollisionGroup /// Return number of ShapeFrames added to this CollisionGroup std::size_t getNumShapeFrames() const; - /// Perform collision detection within this CollisionGroup. - bool collide(const CollisionOption& option, CollisionResult& result); + /// Get the ShapeFrame corresponding to the given index + const dynamics::ShapeFrame* getShapeFrame(std::size_t index) const; - /// Perform collision detection with other CollisionGroup. + /// Perform collision check within this CollisionGroup. + bool collide( + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr); + + /// Perform collision check with other CollisionGroup. /// /// Return false if the engine type of the other CollisionGroup is different /// from this CollisionObject engine. - bool collide(CollisionGroup* group, - const CollisionOption& option, CollisionResult& result); + bool collide( + CollisionGroup* group, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr); protected: diff --git a/dart/collision/Option.cpp b/dart/collision/Option.cpp index e473ac149d5f0..5818bed241d5c 100644 --- a/dart/collision/Option.cpp +++ b/dart/collision/Option.cpp @@ -42,11 +42,9 @@ namespace collision { //============================================================================== CollisionOption::CollisionOption( bool enableContact, - bool binaryCheck, std::size_t maxNumContacts, const std::shared_ptr& collisionFilter) : enableContact(enableContact), - binaryCheck(binaryCheck), maxNumContacts(maxNumContacts), collisionFilter(collisionFilter) { diff --git a/dart/collision/Option.hpp b/dart/collision/Option.hpp index 242f2626471e8..f648bd4e23a9a 100644 --- a/dart/collision/Option.hpp +++ b/dart/collision/Option.hpp @@ -47,26 +47,27 @@ class CollisionFilter; struct CollisionOption { - /// Flag whether compute contact information such as point, normal, and - /// penetration depth. If this flag is set to false, the Engine returns only - /// simple information whether there is a collision of not. - bool enableContact; - /// If true, the collision checking will terminate as soon as the first - /// contact is found. - bool binaryCheck; + /// Flag whether the collision detector computes contact information (contact + /// point, normal, and penetration depth). If it is set to false, only the + /// result of that which pairs are colliding will be stored in the + /// CollisionResult without the contact information. + bool enableContact; - /// Maximum number of contacts to detect + /// Maximum number of contacts to detect. Once the contacts are found up to + /// this number, the collision checking will terminate at that moment. Set + /// this to 1 for binary check. std::size_t maxNumContacts; /// CollisionFilter std::shared_ptr collisionFilter; /// Constructor - CollisionOption(bool enableContact = true, - bool binaryCheck = false, - std::size_t maxNumContacts = 100, - const std::shared_ptr& collisionFilter = nullptr); + CollisionOption( + bool enableContact = true, + std::size_t maxNumContacts = 1000u, + const std::shared_ptr& collisionFilter = nullptr); + }; } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp index f9ef16e219dda..f43692b81d06c 100644 --- a/dart/collision/bullet/BulletCollisionDetector.cpp +++ b/dart/collision/bullet/BulletCollisionDetector.cpp @@ -63,28 +63,24 @@ namespace { struct BulletOverlapFilterCallback : public btOverlapFilterCallback { - BulletOverlapFilterCallback(const CollisionOption& option, const CollisionResult& result) - : mOption(option), - mResult(result), - mDone(false) + BulletOverlapFilterCallback( + const CollisionOption& option, + CollisionResult* result) + : option(option), + result(result), + foundCollision(false), + done(false) { // Do nothing } // return true when pairs need collision - bool needBroadphaseCollision(btBroadphaseProxy* proxy0, - btBroadphaseProxy* proxy1) const override + bool needBroadphaseCollision( + btBroadphaseProxy* proxy0, btBroadphaseProxy* proxy1) const override { - if (mDone) + if (done) return false; - if ((mOption.binaryCheck && mResult.isCollision()) - || (mResult.getNumContacts() >= mOption.maxNumContacts)) - { - mDone = true; - return false; - } - assert((proxy0 != nullptr && proxy1 != nullptr) && "Bullet broadphase overlapping pair proxies are nullptr"); @@ -93,7 +89,7 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback collide = collide && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask); - const auto& filter = mOption.collisionFilter; + const auto& filter = option.collisionFilter; if (collide && filter) { @@ -114,22 +110,28 @@ struct BulletOverlapFilterCallback : public btOverlapFilterCallback return collide; } - const CollisionOption& mOption; - const CollisionResult& mResult; + const CollisionOption& option; + const CollisionResult* result; + + /// True if at least one contact is found. This flag is used only when + /// mResult is nullptr; otherwise the actual collision result is in mResult. + bool foundCollision; /// Whether the collision iteration can stop - mutable bool mDone; + mutable bool done; }; Contact convertContact(const btManifoldPoint& bulletManifoldPoint, const BulletCollisionObject::UserData* userData1, const BulletCollisionObject::UserData* userData2); -void convertContacts( - btCollisionWorld* collWorld, const CollisionOption& option, CollisionResult& result); +void convertContacts(btCollisionWorld* collWorld, + BulletOverlapFilterCallback* overlapFilterCallback, + const CollisionOption& option, + CollisionResult& result); -btCollisionShape* -createBulletEllipsoidMesh(float sizeX, float sizeY, float sizeZ); +btCollisionShape* createBulletEllipsoidMesh( + float sizeX, float sizeY, float sizeZ); btCollisionShape* createBulletCollisionShapeFromAssimpScene( const Eigen::Vector3d& scale, const aiScene* scene); @@ -181,66 +183,136 @@ BulletCollisionDetector::createCollisionGroup() } //============================================================================== -bool BulletCollisionDetector::collide( - CollisionGroup* group, const CollisionOption& option, CollisionResult& result) +static bool checkGroupValidity( + BulletCollisionDetector* cd, CollisionGroup* group) { - result.clear(); - - if (this != group->getCollisionDetector().get()) + if (cd != group->getCollisionDetector().get()) { - dterr << "[BulletCollisionDetector::detect] Attempting to check collision " + dterr << "[BulletCollisionDetector::collide] Attempting to check collision " << "for a collision group that is created from a different collision " << "detector instance.\n"; return false; } - auto castedData = static_cast(group); - castedData->updateEngineData(); + return true; +} + +//============================================================================== +static bool isCollision(btCollisionWorld* world) +{ + assert(world); + + auto dispatcher = world->getDispatcher(); + assert(dispatcher); + + const auto numManifolds = dispatcher->getNumManifolds(); + + for (auto i = 0; i < numManifolds; ++i) + { + const auto* contactManifold = dispatcher->getManifoldByIndexInternal(i); + + if (contactManifold->getNumContacts() > 0) + return true; + } + + return false; +} + +//============================================================================== +bool BulletCollisionDetector::collide( + CollisionGroup* group, + const CollisionOption& option, + CollisionResult* result) +{ + if (result) + result->clear(); + + if (0u == option.maxNumContacts) + return false; + + if (!checkGroupValidity(this, group)) + return false; - auto bulletCollisionWorld = castedData->getBulletCollisionWorld(); + // TODO(JS): It seems, when new BulletOverlapFilterCallback is set to + // btCollisionWorld, bullet doesn't update the list of collision pairs that + // was generated before. Also, there is no way to update the list manually. + // Please report us if it's not true. + // + // In order to have filtered pairs in btCollisionWorld, we instead create a + // new btCollisionWorld (by creating new BulletCollisionGroup) and add the + // collision objects to the new btCollisionWorld so that the filter prevents + // btCollisionWorld to generate unnecessary pairs, which is very inefficient + // way. + + mGroupForFiltering.reset(new BulletCollisionGroup(shared_from_this())); + auto bulletCollisionWorld = mGroupForFiltering->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); auto filterCallback = new BulletOverlapFilterCallback(option, result); - bulletPairCache->setOverlapFilterCallback(filterCallback); + + mGroupForFiltering->addShapeFramesOf(group); + mGroupForFiltering->updateEngineData(); + bulletCollisionWorld->performDiscreteCollisionDetection(); - convertContacts(bulletCollisionWorld, option, result); + if (result) + { + convertContacts(bulletCollisionWorld, filterCallback, option, *result); - return result.isCollision(); + return result->isCollision(); + } + else + { + return isCollision(bulletCollisionWorld); + } } //============================================================================== bool BulletCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) + const CollisionOption& option, CollisionResult* result) { - result.clear(); + if (result) + result->clear(); - if ((this != group1->getCollisionDetector().get()) - || (this != group2->getCollisionDetector().get())) - { - dterr << "[BulletCollisionDetector::detect] Attempting to check collision " - << "for a collision group that is created from a different collision " - << "detector instance.\n"; + if (0u == option.maxNumContacts) + return false; + if (!checkGroupValidity(this, group1)) return false; - } - auto group = common::make_unique(shared_from_this()); - group->addShapeFramesOf(group1, group2); - group->updateEngineData(); + if (!checkGroupValidity(this, group2)) + return false; + + dtwarn << "[BulletCollisionDetector::collide] collide(group1, group2) " + << "supposed to check collisions of the objects in group1 against the " + << "objects in group2. However, the current implementation of this " + << "function checks for all the objects against each other of both " + << "group1 and group2, which is an incorrect behavior. This bug will " + << "be fixed in the next patch release. (see #717 for the details)\n"; - auto bulletCollisionWorld = group->getBulletCollisionWorld(); + mGroupForFiltering.reset(new BulletCollisionGroup(shared_from_this())); + auto bulletCollisionWorld = mGroupForFiltering->getBulletCollisionWorld(); auto bulletPairCache = bulletCollisionWorld->getPairCache(); auto filterCallback = new BulletOverlapFilterCallback(option, result); - bulletPairCache->setOverlapFilterCallback(filterCallback); + + mGroupForFiltering->addShapeFramesOf(group1, group2); + mGroupForFiltering->updateEngineData(); + bulletCollisionWorld->performDiscreteCollisionDetection(); - convertContacts(bulletCollisionWorld, option, result); + if (result) + { + convertContacts(bulletCollisionWorld, filterCallback, option, *result); - return result.isCollision(); + return result->isCollision(); + } + else + { + return isCollision(bulletCollisionWorld); + } } //============================================================================== @@ -455,7 +527,10 @@ Contact convertContact(const btManifoldPoint& bulletManifoldPoint, //============================================================================== void convertContacts( - btCollisionWorld* collWorld, const CollisionOption& option, CollisionResult& result) + btCollisionWorld* collWorld, + BulletOverlapFilterCallback* overlapFilterCallback, + const CollisionOption& option, + CollisionResult& result) { assert(collWorld); @@ -474,9 +549,9 @@ void convertContacts( auto userPointer1 = bulletCollObj1->getUserPointer(); auto userDataA - = static_cast(userPointer1); - auto userDataB = static_cast(userPointer0); + auto userDataB + = static_cast(userPointer1); auto numContacts = contactManifold->getNumContacts(); @@ -486,11 +561,11 @@ void convertContacts( result.addContact(convertContact(cp, userDataA, userDataB)); - if (option.binaryCheck) - return; - if (result.getNumContacts() >= option.maxNumContacts) - break; + { + overlapFilterCallback->done = true; + return; + } } } } diff --git a/dart/collision/bullet/BulletCollisionDetector.hpp b/dart/collision/bullet/BulletCollisionDetector.hpp index d482abeebe962..666acb94f3a59 100644 --- a/dart/collision/bullet/BulletCollisionDetector.hpp +++ b/dart/collision/bullet/BulletCollisionDetector.hpp @@ -44,6 +44,7 @@ #include #include #include "dart/collision/CollisionDetector.hpp" +#include "dart/collision/bullet/BulletCollisionGroup.hpp" namespace dart { namespace collision { @@ -74,12 +75,17 @@ class BulletCollisionDetector : public CollisionDetector std::unique_ptr createCollisionGroup() override; // Documentation inherited - bool collide(CollisionGroup* group, - const CollisionOption& option, CollisionResult& result) override; + bool collide( + CollisionGroup* group, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) override; // Documentation inherited - bool collide(CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) override; + bool collide( + CollisionGroup* group1, + CollisionGroup* group2, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) override; protected: @@ -109,6 +115,8 @@ class BulletCollisionDetector : public CollisionDetector std::map> mShapeMap; + std::unique_ptr mGroupForFiltering; + }; } // namespace collision diff --git a/dart/collision/bullet/BulletCollisionGroup.cpp b/dart/collision/bullet/BulletCollisionGroup.cpp index 5910cfc0060dd..50bddd7704029 100644 --- a/dart/collision/bullet/BulletCollisionGroup.cpp +++ b/dart/collision/bullet/BulletCollisionGroup.cpp @@ -117,7 +117,13 @@ void BulletCollisionGroup::updateCollisionGroupEngineData() } //============================================================================== -btCollisionWorld* BulletCollisionGroup::getBulletCollisionWorld() const +btCollisionWorld* BulletCollisionGroup::getBulletCollisionWorld() +{ + return mBulletCollisionWorld.get(); +} + +//============================================================================== +const btCollisionWorld* BulletCollisionGroup::getBulletCollisionWorld() const { return mBulletCollisionWorld.get(); } diff --git a/dart/collision/bullet/BulletCollisionGroup.hpp b/dart/collision/bullet/BulletCollisionGroup.hpp index f18e937ea62b3..64521cb0dc23d 100644 --- a/dart/collision/bullet/BulletCollisionGroup.hpp +++ b/dart/collision/bullet/BulletCollisionGroup.hpp @@ -81,7 +81,10 @@ class BulletCollisionGroup : public CollisionGroup void updateCollisionGroupEngineData() override; /// Return Bullet collision world - btCollisionWorld* getBulletCollisionWorld() const; + btCollisionWorld* getBulletCollisionWorld(); + + /// Return Bullet collision world + const btCollisionWorld* getBulletCollisionWorld() const; protected: diff --git a/dart/collision/dart/DARTCollide.cpp b/dart/collision/dart/DARTCollide.cpp index 4c53683f1c300..4771287fb6aaa 100644 --- a/dart/collision/dart/DARTCollide.cpp +++ b/dart/collision/dart/DARTCollide.cpp @@ -1255,6 +1255,9 @@ int collideCylinderPlane(CollisionObject* o1, CollisionObject* o2, const double& //============================================================================== int collide(CollisionObject* o1, CollisionObject* o2, CollisionResult& result) { + // TODO(JS): We could make the contact point computation as optional for + // the case that we want only binary check. + const dynamics::ConstShapePtr& shape0 = o1->getShape(); const dynamics::ConstShapePtr& shape1 = o2->getShape(); const Eigen::Isometry3d& T0 = o1->getTransform(); diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp index 52062c1c22d7c..aa743c263f156 100644 --- a/dart/collision/dart/DARTCollisionDetector.cpp +++ b/dart/collision/dart/DARTCollisionDetector.cpp @@ -50,7 +50,7 @@ namespace collision { namespace { bool checkPair(CollisionObject* o1, CollisionObject* o2, - const CollisionOption& option, CollisionResult& result); + const CollisionOption& option, CollisionResult* result = nullptr); bool isClose(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, double tol); @@ -96,75 +96,93 @@ DARTCollisionDetector::createCollisionGroup() } //============================================================================== -bool DARTCollisionDetector::collide( - CollisionGroup* group, - const CollisionOption& option, CollisionResult& result) +static bool checkGroupValidity(DARTCollisionDetector* cd, CollisionGroup* group) { - result.clear(); - - if (this != group->getCollisionDetector().get()) + if (cd != group->getCollisionDetector().get()) { - dterr << "[DARTCollisionDetector::detect] Attempting to check collision " + dterr << "[DARTCollisionDetector::collide] Attempting to check collision " << "for a collision group that is created from a different collision " << "detector instance.\n"; return false; } + return true; +} + +//============================================================================== +bool DARTCollisionDetector::collide( + CollisionGroup* group, + const CollisionOption& option, + CollisionResult* result) +{ + if (result) + result->clear(); + + if (0u == option.maxNumContacts) + return false; + + if (!checkGroupValidity(this, group)) + return false; + auto casted = static_cast(group); const auto& objects = casted->mCollisionObjects; if (objects.empty()) return false; - auto done = false; + auto collisionFound = false; const auto& filter = option.collisionFilter; for (auto i = 0u; i < objects.size() - 1; ++i) { - auto collObj1 = objects[i]; + auto* collObj1 = objects[i]; for (auto j = i + 1u; j < objects.size(); ++j) { - auto collObj2 = objects[j]; + auto* collObj2 = objects[j]; if (filter && !filter->needCollision(collObj1, collObj2)) continue; - checkPair(collObj1, collObj2, option, result); + collisionFound = checkPair(collObj1, collObj2, option, result); - if ((option.binaryCheck && result.isCollision()) - || (result.getNumContacts() >= option.maxNumContacts)) + if (result) { - done = true; - break; + if (result->getNumContacts() >= option.maxNumContacts) + return true; + } + else + { + // If no result is passed, stop checking when the first contact is found + if (collisionFound) + return true; } } - - if (done) - break; } - return result.isCollision(); + // Either no collision found or not reached the maximum number of contacts + return collisionFound; } //============================================================================== bool DARTCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) + const CollisionOption& option, + CollisionResult* result) { - result.clear(); + if (result) + result->clear(); - if ((this != group1->getCollisionDetector().get()) - || (this != group2->getCollisionDetector().get())) - { - dterr << "[DARTCollisionDetector::detect] Attempting to check collision " - << "for a collision group that is created from a different collision " - << "detector instance.\n"; + if (0u == option.maxNumContacts) + return false; + if (!checkGroupValidity(this, group1)) + return false; + + if (!checkGroupValidity(this, group2)) return false; - } auto casted1 = static_cast(group1); auto casted2 = static_cast(group2); @@ -175,34 +193,38 @@ bool DARTCollisionDetector::collide( if (objects1.empty() || objects2.empty()) return false; - auto done = false; + auto collisionFound = false; const auto& filter = option.collisionFilter; for (auto i = 0u; i < objects1.size(); ++i) { - auto collObj1 = objects1[i]; + auto* collObj1 = objects1[i]; for (auto j = 0u; j < objects2.size(); ++j) { - auto collObj2 = objects2[j]; + auto* collObj2 = objects2[j]; if (filter && !filter->needCollision(collObj1, collObj2)) continue; - checkPair(collObj1, collObj2, option, result); + collisionFound = checkPair(collObj1, collObj2, option, result); - if (result.getNumContacts() >= option.maxNumContacts) + if (result) { - done = true; - break; + if (result->getNumContacts() >= option.maxNumContacts) + return true; + } + else + { + // If no result is passed, stop checking when the first contact is found + if (collisionFound) + return true; } } - - if (done) - break; } - return result.isCollision(); + // Either no collision found or not reached the maximum number of contacts + return collisionFound; } //============================================================================== @@ -256,16 +278,20 @@ namespace { //============================================================================== bool checkPair(CollisionObject* o1, CollisionObject* o2, - const CollisionOption& option, CollisionResult& result) + const CollisionOption& option, CollisionResult* result) { CollisionResult pairResult; // Perform narrow-phase detection - auto colliding = collide(o1, o2, pairResult); + collide(o1, o2, pairResult); + + // Early return for binary check + if (!result) + return pairResult.isCollision(); - postProcess(o1, o2, option, result, pairResult); + postProcess(o1, o2, option, *result, pairResult); - return colliding != 0; + return pairResult.isCollision(); } //============================================================================== @@ -276,9 +302,11 @@ bool isClose(const Eigen::Vector3d& pos1, const Eigen::Vector3d& pos2, } //============================================================================== -void postProcess(CollisionObject* o1, CollisionObject* o2, +void postProcess(CollisionObject* o1, + CollisionObject* o2, const CollisionOption& option, - CollisionResult& totalResult, const CollisionResult& pairResult) + CollisionResult& totalResult, + const CollisionResult& pairResult) { if (!pairResult.isCollision()) return; @@ -307,9 +335,6 @@ void postProcess(CollisionObject* o1, CollisionObject* o2, contact.collisionObject2 = o2; totalResult.addContact(contact); - if (option.binaryCheck) - break; - if (totalResult.getNumContacts() >= option.maxNumContacts) break; } diff --git a/dart/collision/dart/DARTCollisionDetector.hpp b/dart/collision/dart/DARTCollisionDetector.hpp index 77b78e1ac1f13..ef379334aa62f 100644 --- a/dart/collision/dart/DARTCollisionDetector.hpp +++ b/dart/collision/dart/DARTCollisionDetector.hpp @@ -64,12 +64,17 @@ class DARTCollisionDetector : public CollisionDetector std::unique_ptr createCollisionGroup() override; // Documentation inherited - bool collide(CollisionGroup* group, - const CollisionOption& option, CollisionResult& result) override; + bool collide( + CollisionGroup* group, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) override; // Documentation inherited - bool collide(CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) override; + bool collide( + CollisionGroup* group1, + CollisionGroup* group2, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) override; protected: diff --git a/dart/collision/detail/CollisionGroup.hpp b/dart/collision/detail/CollisionGroup.hpp index da306652982f4..1f26ac897ff36 100644 --- a/dart/collision/detail/CollisionGroup.hpp +++ b/dart/collision/detail/CollisionGroup.hpp @@ -101,7 +101,7 @@ void CollisionGroup::addShapeFramesOf( //============================================================================== template void CollisionGroup::addShapeFramesOf( - const dynamics::Skeleton* skel, const Others*... others) + const dynamics::MetaSkeleton* skel, const Others*... others) { assert(skel); diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp index 8f2f243d255c7..9a38b44f8130a 100644 --- a/dart/collision/fcl/FCLCollisionDetector.cpp +++ b/dart/collision/fcl/FCLCollisionDetector.cpp @@ -68,33 +68,48 @@ namespace collision { namespace { -bool collisionCallback(fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - void* cdata); - -void postProcessFCL(const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - const CollisionOption& option, - CollisionResult& result); - -void postProcessDART(const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - const CollisionOption& option, - CollisionResult& result); +bool collisionCallback( + fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata); + +void postProcessFCL( + const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const CollisionOption& option, + CollisionResult& result); + +void postProcessDART( + const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const CollisionOption& option, + CollisionResult& result); int evalContactPosition(const fcl::Contact& fclContact, const fcl::BVHModel* mesh1, const fcl::BVHModel* mesh2, const fcl::Transform3f& transform1, const fcl::Transform3f& transform2, - Eigen::Vector3d* contactPosition); + Eigen::Vector3d& contactPosition1, Eigen::Vector3d& contactPosition2); + +Eigen::Vector3d getDiff(const Contact& contact1, const Contact& contact2); + +fcl::Vec3f getDiff(const fcl::Contact& contact1, const fcl::Contact& contact2); -bool isColinear(const Eigen::Vector3d& pos1, - const Eigen::Vector3d& pos2, - const Eigen::Vector3d& pos3, - double tol); +bool isColinear( + const Contact& contact1, + const Contact& contact2, + const Contact& contact3, + double tol); + +bool isColinear( + const fcl::Contact& contact1, + const fcl::Contact& contact2, + const fcl::Contact& contact3, + double tol); + +template +bool isColinear(const T& pos1, const T& pos2, const T& pos3, double tol); int FFtest( const fcl::Vec3f& r1, const fcl::Vec3f& r2, const fcl::Vec3f& r3, @@ -103,54 +118,70 @@ int FFtest( double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3); -void convertOption(const CollisionOption& fclOption, fcl::CollisionRequest& request); +void convertOption( + const CollisionOption& fclOption, fcl::CollisionRequest& request); -Contact convertContact(const fcl::Contact& fclContact, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2); +Contact convertContact( + const fcl::Contact& fclContact, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const CollisionOption& option); /// Collision data stores the collision request and the result given by /// collision algorithm. struct FCLCollisionCallbackData { /// FCL collision request - fcl::CollisionRequest mFclRequest; + fcl::CollisionRequest fclRequest; /// FCL collision result - fcl::CollisionResult mFclResult; + fcl::CollisionResult fclResult; /// Collision option of DART - const CollisionOption& mOption; + const CollisionOption& option; /// Collision result of DART - CollisionResult& mResult; + CollisionResult* result; + + /// True if at least one contact is found. This flag is used only when + /// mResult is nullptr; otherwise the actual collision result is in mResult. + bool foundCollision; - FCLCollisionDetector::PrimitiveShape mPrimitiveShapeType; + FCLCollisionDetector::PrimitiveShape primitiveShapeType; FCLCollisionDetector::ContactPointComputationMethod - mContactPointComputationMethod; + contactPointComputationMethod; /// Whether the collision iteration can stop bool done; + bool isCollision() const + { + if (result) + return result->isCollision(); + else + return foundCollision; + } + /// Constructor FCLCollisionCallbackData( const CollisionOption& option, - CollisionResult& result, + CollisionResult* result, FCLCollisionDetector::PrimitiveShape type = FCLCollisionDetector::MESH, FCLCollisionDetector::ContactPointComputationMethod method = FCLCollisionDetector::DART) - : mOption(option), - mResult(result), - mPrimitiveShapeType(type), - mContactPointComputationMethod(method), + : option(option), + result(result), + foundCollision(false), + primitiveShapeType(type), + contactPointComputationMethod(method), done(false) { - convertOption(mOption, mFclRequest); + convertOption(option, fclRequest); - mFclRequest.num_max_contacts = std::max(static_cast(100u), - mOption.maxNumContacts); + fclRequest.num_max_contacts = std::max(static_cast(100u), + option.maxNumContacts); // Since some contact points can be filtered out in the post process, we ask // more than the demend. 100 is randomly picked. } @@ -613,64 +644,79 @@ FCLCollisionDetector::createCollisionGroup() } //============================================================================== -bool FCLCollisionDetector::collide( - CollisionGroup* group, const CollisionOption& option, CollisionResult& result) +static bool checkGroupValidity(FCLCollisionDetector* cd, CollisionGroup* group) { - result.clear(); - - if (this != group->getCollisionDetector().get()) + if (cd != group->getCollisionDetector().get()) { - dterr << "[FCLCollisionDetector::detect] Attempting to check collision " + dterr << "[FCLCollisionDetector::collide] Attempting to check collision " << "for a collision group that is created from a different collision " << "detector instance.\n"; return false; } + return true; +} + +//============================================================================== +bool FCLCollisionDetector::collide( + CollisionGroup* group, + const CollisionOption& option, + CollisionResult* result) +{ + if (result) + result->clear(); + + if (0u == option.maxNumContacts) + return false; + + if (!checkGroupValidity(this, group)) + return false; + auto casted = static_cast(group); casted->updateEngineData(); - auto broadPhaseAlg = casted->getFCLCollisionManager(); - FCLCollisionCallbackData collData( option, result, mPrimitiveShapeType, mContactPointComputationMethod); - broadPhaseAlg->collide(&collData, collisionCallback); - return result.isCollision(); + casted->getFCLCollisionManager()->collide(&collData, collisionCallback); + + return collData.isCollision(); } //============================================================================== bool FCLCollisionDetector::collide( CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) + const CollisionOption& option, CollisionResult* result) { - result.clear(); + if (result) + result->clear(); - if ((this != group1->getCollisionDetector().get()) - || (this != group2->getCollisionDetector().get())) - { - dterr << "[FCLCollisionDetector::detect] Attempting to check collision " - << "for a collision group that is created from a different collision " - << "detector instance.\n"; + if (0u == option.maxNumContacts) + return false; + if (!checkGroupValidity(this, group1)) + return false; + + if (!checkGroupValidity(this, group2)) return false; - } auto casted1 = static_cast(group1); auto casted2 = static_cast(group2); casted1->updateEngineData(); casted2->updateEngineData(); - auto broadPhaseAlg1 = casted1->getFCLCollisionManager(); - auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); - FCLCollisionCallbackData collData( option, result, mPrimitiveShapeType, mContactPointComputationMethod); + + auto broadPhaseAlg1 = casted1->getFCLCollisionManager(); + auto broadPhaseAlg2 = casted2->getFCLCollisionManager(); + broadPhaseAlg1->collide(broadPhaseAlg2, &collData, collisionCallback); - return result.isCollision(); + return collData.isCollision(); } //============================================================================== @@ -936,16 +982,19 @@ namespace { bool collisionCallback( fcl::CollisionObject* o1, fcl::CollisionObject* o2, void* cdata) { + // Return true if you don't want more narrow phase collision checking after + // this callback function returns, return false otherwise. + auto collData = static_cast(cdata); if (collData->done) return true; - const auto& fclRequest = collData->mFclRequest; - auto& fclResult = collData->mFclResult; - auto& result = collData->mResult; - const auto& option = collData->mOption; - auto filter = option.collisionFilter; + const auto& fclRequest = collData->fclRequest; + auto& fclResult = collData->fclResult; + auto* result = collData->result; + const auto& option = collData->option; + const auto& filter = option.collisionFilter; // Filtering if (filter) @@ -972,143 +1021,195 @@ bool collisionCallback( // Perform narrow-phase detection fcl::collide(o1, o2, fclRequest, fclResult); - if (FCLCollisionDetector::DART == collData->mContactPointComputationMethod - && FCLCollisionDetector::MESH == collData->mPrimitiveShapeType) + if (result) { - postProcessDART(fclResult, o1, o2, option, result); + // Post processing -- converting fcl contact information to ours if needed + if (FCLCollisionDetector::DART == collData->contactPointComputationMethod + && FCLCollisionDetector::MESH == collData->primitiveShapeType) + { + postProcessDART(fclResult, o1, o2, option, *result); + } + else + { + postProcessFCL(fclResult, o1, o2, option, *result); + } + + // Check satisfaction of the stopping conditions + if (result->getNumContacts() >= option.maxNumContacts) + collData->done = true; } else { - postProcessFCL(fclResult, o1, o2, option, result); - } - - if ((option.binaryCheck && result.isCollision()) - || (result.getNumContacts() >= option.maxNumContacts)) - { - collData->done = true; - return true; + // If no result is passed, stop checking when the first contact is found + if (fclResult.isCollision()) + { + collData->foundCollision = true; + collData->done = true; + } } return collData->done; } //============================================================================== -void postProcessFCL(const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - const CollisionOption& option, - CollisionResult& result) +Eigen::Vector3d getDiff(const Contact& contact1, const Contact& contact2) { - auto numContacts = fclResult.numContacts(); - - if (0 == numContacts) - return; + return contact1.point - contact2.point; +} - const double ZERO = 0.000001; - const double ZERO2 = ZERO*ZERO; +//============================================================================== +fcl::Vec3f getDiff(const fcl::Contact& contact1, const fcl::Contact& contact2) +{ + return contact1.pos - contact2.pos; +} - std::vector markForDeletion(numContacts, false); +//============================================================================== +template +void markRepeatedPoints( + std::vector& markForDeletion, + const ResultT& fclResult, + double tol) +{ + const auto checkSize = markForDeletion.size(); - // mark all the repeated points - for (auto i = 0u; i < numContacts - 1; ++i) + for (auto i = 0u; i < checkSize - 1u; ++i) { - const auto& contact1 = fclResult.getContact(i); + const auto& contact1 = (fclResult.*GetFun)(i); - for (auto j = i + 1u; j < numContacts; ++j) + for (auto j = i + 1u; j < checkSize; ++j) { - const auto& contact2 = fclResult.getContact(j); + const auto& contact2 = (fclResult.*GetFun)(j); - const auto diff = contact1.pos - contact2.pos; + const auto diff = getDiff(contact1, contact2); - if (diff.length() < 3 * ZERO2) + if (diff.dot(diff) < tol) { markForDeletion[i] = true; break; } } } +} - // remove all the co-linear contact points - for (auto i = 0u; i < numContacts; ++i) +//============================================================================== +template +void markColinearPoints( + std::vector& markForDeletion, + const ResultT& fclResult, + double tol) +{ + const auto checkSize = markForDeletion.size(); + + for (auto i = 0u; i < checkSize; ++i) { if (markForDeletion[i]) continue; - const auto& contact1 = fclResult.getContact(i); + const auto& contact1 = (fclResult.*GetFun)(i); - for (auto j = i + 1u; j < numContacts; ++j) + for (auto j = i + 1u; j < checkSize; ++j) { - if (markForDeletion[j]) + if (i == j || markForDeletion[j]) continue; - const auto& contact2 = fclResult.getContact(j); + if (markForDeletion[i]) + break; + + const auto& contact2 = (fclResult.*GetFun)(j); - for (auto k = j + 1u; k < numContacts; ++k) + for (auto k = j + 1u; k < checkSize; ++k) { - if (markForDeletion[k]) + if (i == k) continue; - const auto& contact3 = fclResult.getContact(k); - - const auto va = contact1.pos - contact2.pos; - const auto vb = contact1.pos - contact3.pos; - const auto v = va.cross(vb); + const auto& contact3 = (fclResult.*GetFun)(k); - if (v.length() < ZERO2) + if (isColinear(contact1, contact2, contact3, tol)) { markForDeletion[i] = true; break; } } } + } +} - if (option.binaryCheck) - { - const auto fclContact = fclResult.getContact(i); - convertContact(fclContact, o1, o2); +//============================================================================== +void postProcessFCL( + const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const CollisionOption& option, + CollisionResult& result) +{ + const auto numContacts = fclResult.numContacts(); - return; - } + if (0u == numContacts) + return; + + // For binary check, return after adding the first contact point to the result + // without the checkings of repeatidity and co-linearity. + if (1u == option.maxNumContacts) + { + result.addContact(convertContact(fclResult.getContact(0), o1, o2, option)); + + return; } - for (std::size_t i = 0; i < numContacts; ++i) + const auto tol = 1e-12; + const auto tol3 = tol * 3.0; + + std::vector markForDeletion(numContacts, false); + + // mark all the repeated points + markRepeatedPoints< + fcl::CollisionResult, + fcl::Contact, + &fcl::CollisionResult::getContact>(markForDeletion, fclResult, tol3); + + // remove all the co-linear contact points + markColinearPoints< + fcl::CollisionResult, + fcl::Contact, + &fcl::CollisionResult::getContact>(markForDeletion, fclResult, tol); + + for (auto i = 0u; i < numContacts; ++i) { - if (!markForDeletion[i]) - { - const auto fclContact = fclResult.getContact(i); - result.addContact(convertContact(fclContact, o1, o2)); - } + if (markForDeletion[i]) + continue; + + result.addContact(convertContact(fclResult.getContact(i), o1, o2, option)); if (result.getNumContacts() >= option.maxNumContacts) - break; + return; } } //============================================================================== -void postProcessDART(const fcl::CollisionResult& fclResult, - fcl::CollisionObject* o1, - fcl::CollisionObject* o2, - const CollisionOption& option, - CollisionResult& result) +void postProcessDART( + const fcl::CollisionResult& fclResult, + fcl::CollisionObject* o1, + fcl::CollisionObject* o2, + const CollisionOption& option, + CollisionResult& result) { - auto initNumContacts = fclResult.numContacts(); + const auto numFilteredContacts = fclResult.numContacts(); - if (0 == initNumContacts) + if (0u == numFilteredContacts) return; - int numCoplanarContacts = 0; - int numNoContacts = 0; - int numContacts = 0; + auto numContacts = 0u; std::vector unfiltered; - unfiltered.reserve(initNumContacts * 2); + unfiltered.reserve(numFilteredContacts * 2); - for (auto k = 0u; k < initNumContacts; ++k) + for (auto i = 0u; i < numFilteredContacts; ++i) { - // for each pair of intersecting triangles, we create two contact points - Contact pair1; - Contact pair2; - const auto& c = fclResult.getContact(k); + const auto& c = fclResult.getContact(i); auto userData1 = static_cast(o1->getUserData()); @@ -1117,119 +1218,86 @@ void postProcessDART(const fcl::CollisionResult& fclResult, assert(userData1); assert(userData2); - auto collisionObject1 = userData1->mCollisionObject; - auto collisionObject2 = userData2->mCollisionObject; + // for each pair of intersecting triangles, we create two contact points + Contact pair1; + Contact pair2; - int contactResult = evalContactPosition( - c, - static_cast*>(c.o1), - static_cast*>(c.o2), - FCLTypes::convertTransform(collisionObject1->getTransform()), - FCLTypes::convertTransform(collisionObject2->getTransform()), - &pair1.point); + pair1.collisionObject1 = userData1->mCollisionObject; + pair1.collisionObject2 = userData2->mCollisionObject; - if (contactResult == COPLANAR_CONTACT) + if (option.enableContact) { - numCoplanarContacts++; - - if (numContacts > 2) + pair1.normal = FCLTypes::convertVector3(-c.normal); + pair1.penetrationDepth = c.penetration_depth; + pair1.triID1 = c.b1; + pair1.triID2 = c.b2; + pair2 = pair1; + + auto contactResult = evalContactPosition( + c, + static_cast*>(c.o1), + static_cast*>(c.o2), + FCLTypes::convertTransform(pair1.collisionObject1->getTransform()), + FCLTypes::convertTransform(pair1.collisionObject2->getTransform()), + pair1.point, + pair2.point); + + if (contactResult == COPLANAR_CONTACT) + { + if (numContacts > 2u) + continue; + } + else if (contactResult == NO_CONTACT) + { continue; + } + else + { + numContacts++; + } } - else if (contactResult == NO_CONTACT) - { - numNoContacts++; - continue; - } - else + // For binary check, return after adding the first contact point to the result + // without the checkings of repeatidity and co-linearity. + if (1u == option.maxNumContacts) { - numContacts++; - } + result.addContact(pair1); - pair1.normal = FCLTypes::convertVector3(-c.normal); - pair1.penetrationDepth = c.penetration_depth; - pair1.triID1 = c.b1; - pair1.triID2 = c.b2; - pair1.collisionObject1 = collisionObject1; - pair1.collisionObject2 = collisionObject2; + return; + } - pair2 = pair1; unfiltered.push_back(pair1); unfiltered.push_back(pair2); } const auto tol = 1e-12; + const auto tol3 = tol * 3.0; - auto unfilteredSize = unfiltered.size(); + const auto unfilteredSize = unfiltered.size(); std::vector markForDeletion(unfilteredSize, false); // mark all the repeated points - for (auto i = 0u; i < unfilteredSize - 1u; ++i) - { - const auto& contact1 = unfiltered[i]; - - for (auto j = i + 1u; j < unfilteredSize; ++j) - { - const auto& contact2 = unfiltered[j]; - - const auto diff = contact1.point - contact2.point; - - if (diff.dot(diff) < 3.0 * tol) - { - markForDeletion[i] = true; - break; - } - } - } + markRepeatedPoints< + std::vector, + Contact, + &std::vector::at>(markForDeletion, unfiltered, tol3); // remove all the co-linear contact points + markColinearPoints< + std::vector, + Contact, + &std::vector::at>(markForDeletion, unfiltered, tol); + for (auto i = 0u; i < unfilteredSize; ++i) { if (markForDeletion[i]) continue; - const auto& contact1 = unfiltered[i]; - - for (auto j = 0u; j < unfilteredSize; ++j) - { - if (i == j || markForDeletion[j]) - continue; - - if (markForDeletion[i]) - break; - - const auto& contact2 = unfiltered[j]; - - for (auto k = j + 1u; k < unfilteredSize; ++k) - { - if (i == k) - continue; - - const auto& contact3 = unfiltered[k]; - - if (isColinear(contact1.point, contact2.point, contact3.point, tol)) - { - markForDeletion[i] = true; - break; - } - } - } - - if (option.binaryCheck) - { - result.addContact(unfiltered[i]); - return; - } - } - - for (auto i = 0u; i < unfilteredSize; ++i) - { - if (!markForDeletion[i]) - result.addContact(unfiltered[i]); + result.addContact(unfiltered[i]); if (result.getNumContacts() >= option.maxNumContacts) - break; + return; } } @@ -1240,12 +1308,13 @@ int evalContactPosition( const fcl::BVHModel* mesh2, const fcl::Transform3f& transform1, const fcl::Transform3f& transform2, - Eigen::Vector3d* contactPosition) + Eigen::Vector3d& contactPosition1, + Eigen::Vector3d& contactPosition2) { - int id1 = fclContact.b1; - int id2 = fclContact.b2; - fcl::Triangle tri1 = mesh1->tri_indices[id1]; - fcl::Triangle tri2 = mesh2->tri_indices[id2]; + auto id1 = fclContact.b1; + auto id2 = fclContact.b2; + auto tri1 = mesh1->tri_indices[id1]; + auto tri2 = mesh2->tri_indices[id2]; fcl::Vec3f v1, v2, v3, p1, p2, p3; v1 = mesh1->vertices[tri1[0]]; @@ -1263,12 +1332,12 @@ int evalContactPosition( p1 = transform2.transform(p1); p2 = transform2.transform(p2); p3 = transform2.transform(p3); - int testRes = FFtest(v1, v2, v3, p1, p2, p3, &contact1, &contact2); + auto testRes = FFtest(v1, v2, v3, p1, p2, p3, &contact1, &contact2); if (testRes == COPLANAR_CONTACT) { - double area1 = triArea(v1, v2, v3); - double area2 = triArea(p1, p2, p3); + auto area1 = triArea(v1, v2, v3); + auto area2 = triArea(p1, p2, p3); if (area1 < area2) contact1 = v1 + v2 + v3; @@ -1281,7 +1350,8 @@ int evalContactPosition( contact2 = contact1; } - *contactPosition = Eigen::Vector3d(contact1[0], contact1[1], contact1[2]); + contactPosition1 << contact1[0], contact1[1], contact1[2]; + contactPosition2 << contact2[0], contact2[1], contact2[2]; return testRes; } @@ -1322,10 +1392,28 @@ double triArea(fcl::Vec3f& p1, fcl::Vec3f& p2, fcl::Vec3f& p3) } //============================================================================== -bool isColinear(const Eigen::Vector3d& pos1, - const Eigen::Vector3d& pos2, - const Eigen::Vector3d& pos3, - double tol) +bool isColinear( + const Contact& contact1, + const Contact& contact2, + const Contact& contact3, + double tol) +{ + return isColinear(contact1.point, contact2.point, contact3.point, tol); +} + +//============================================================================== +bool isColinear( + const fcl::Contact& contact1, + const fcl::Contact& contact2, + const fcl::Contact& contact3, + double tol) +{ + return isColinear(contact1.pos, contact2.pos, contact3.pos, tol); +} + +//============================================================================== +template +bool isColinear(const T& pos1, const T& pos2, const T& pos3, double tol) { const auto va = pos1 - pos2; const auto vb = pos1 - pos3; @@ -1350,18 +1438,11 @@ void convertOption(const CollisionOption& fclOption, fcl::CollisionRequest& requ //============================================================================== Contact convertContact(const fcl::Contact& fclContact, fcl::CollisionObject* o1, - fcl::CollisionObject* o2) + fcl::CollisionObject* o2, + const CollisionOption& option) { Contact contact; - Eigen::Vector3d point = FCLTypes::convertVector3(fclContact.pos); - - contact.point = point; - contact.normal = -FCLTypes::convertVector3(fclContact.normal); - contact.penetrationDepth = fclContact.penetration_depth; - contact.triID1 = fclContact.b1; - contact.triID2 = fclContact.b2; - auto userData1 = static_cast(o1->getUserData()); auto userData2 @@ -1371,6 +1452,15 @@ Contact convertContact(const fcl::Contact& fclContact, contact.collisionObject1 = userData1->mCollisionObject; contact.collisionObject2 = userData2->mCollisionObject; + if (option.enableContact) + { + contact.point = FCLTypes::convertVector3(fclContact.pos); + contact.normal = -FCLTypes::convertVector3(fclContact.normal); + contact.penetrationDepth = fclContact.penetration_depth; + contact.triID1 = fclContact.b1; + contact.triID2 = fclContact.b2; + } + return contact; } diff --git a/dart/collision/fcl/FCLCollisionDetector.hpp b/dart/collision/fcl/FCLCollisionDetector.hpp index 734b69430f9e9..f7f90ab625e90 100644 --- a/dart/collision/fcl/FCLCollisionDetector.hpp +++ b/dart/collision/fcl/FCLCollisionDetector.hpp @@ -101,12 +101,17 @@ class FCLCollisionDetector : public CollisionDetector std::unique_ptr createCollisionGroup() override; // Documentation inherited - bool collide(CollisionGroup* group, - const CollisionOption& option, CollisionResult& result) override; + bool collide( + CollisionGroup* group, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) override; // Documentation inherited - bool collide(CollisionGroup* group1, CollisionGroup* group2, - const CollisionOption& option, CollisionResult& result) override; + bool collide( + CollisionGroup* group1, + CollisionGroup* group2, + const CollisionOption& option = CollisionOption(false, 1u, nullptr), + CollisionResult* result = nullptr) override; /// Set primitive shape type void setPrimitiveShapeType(PrimitiveShape type); diff --git a/dart/common/Aspect.cpp b/dart/common/Aspect.cpp index 1a88d8b735fe6..ffa6bebb45a77 100644 --- a/dart/common/Aspect.cpp +++ b/dart/common/Aspect.cpp @@ -68,17 +68,6 @@ const Aspect::Properties* Aspect::getAspectProperties() const return nullptr; } -//============================================================================== -Aspect::Aspect(Composite* composite) -{ - if(nullptr == composite) - { - dterr << "[Aspect::constructor] You are not allowed to construct an Aspect " - << "outside of an Composite!\n"; - assert(false); - } -} - //============================================================================== void Aspect::setComposite(Composite* /*newComposite*/) { diff --git a/dart/common/Aspect.hpp b/dart/common/Aspect.hpp index 5a54d34a4c9e3..a70d75aa40094 100644 --- a/dart/common/Aspect.hpp +++ b/dart/common/Aspect.hpp @@ -95,7 +95,7 @@ class Aspect virtual ~Aspect() = default; /// Clone this Aspect into a new composite - virtual std::unique_ptr cloneAspect(Composite* newComposite) const = 0; + virtual std::unique_ptr cloneAspect() const = 0; /// Set the State of this Aspect. By default, this does nothing. virtual void setAspectState(const State& otherState); @@ -113,13 +113,6 @@ class Aspect protected: - /// Constructor - /// - /// We require the Composite argument in this constructor to make it clear - /// to extensions that they must have a Composite argument in their - /// constructors. - Aspect(Composite* composite); - /// This function will be triggered (1) after the Aspect has been created /// [transfer will be false] and (2) after the Aspect has been transferred /// to a new Composite [transfer will be true]. You should override this @@ -141,7 +134,7 @@ class CompositeTrackingAspect : public Aspect public: /// Default constructor - CompositeTrackingAspect(Composite* comp); + CompositeTrackingAspect(); /// Get the Composite of this Aspect CompositeType* getComposite(); @@ -171,16 +164,16 @@ class CompositeTrackingAspect : public Aspect //============================================================================== #define DART_COMMON_ASPECT_PROPERTY_CONSTRUCTOR( ClassName, UpdatePropertiesMacro )\ ClassName (const ClassName &) = delete;\ - inline ClassName (dart::common::Composite* comp, const PropertiesData& properties = PropertiesData())\ - : AspectWithVersionedProperties< Base, Derived, PropertiesData, CompositeType, UpdatePropertiesMacro>(comp, properties) { } + inline ClassName (const PropertiesData& properties = PropertiesData())\ + : AspectWithVersionedProperties< Base, Derived, PropertiesData, CompositeType, UpdatePropertiesMacro>(properties) { } //============================================================================== #define DART_COMMON_ASPECT_STATE_PROPERTY_CONSTRUCTORS(ClassName)\ ClassName (const ClassName &) = delete;\ - inline ClassName (dart::common::Composite* comp, const StateData& state = StateData(), const PropertiesData& properties = PropertiesData())\ - : AspectImpl(comp, state, properties) { }\ - inline ClassName (dart::common::Composite* comp, const PropertiesData& properties, const StateData state = StateData())\ - : AspectImpl(comp, properties, state) { } + inline ClassName (const StateData& state = StateData(), const PropertiesData& properties = PropertiesData())\ + : AspectImpl(state, properties) { }\ + inline ClassName (const PropertiesData& properties, const StateData state = StateData())\ + : AspectImpl(properties, state) { } //============================================================================== #define DART_COMMON_SET_ASPECT_PROPERTY_CUSTOM( Type, Name, Update )\ diff --git a/dart/common/AspectWithVersion.hpp b/dart/common/AspectWithVersion.hpp index 9a0fee3b1c369..d357679e6df29 100644 --- a/dart/common/AspectWithVersion.hpp +++ b/dart/common/AspectWithVersion.hpp @@ -98,29 +98,26 @@ class AspectWithStateAndVersionedProperties : /// Construct using a StateData and a PropertiesData instance AspectWithStateAndVersionedProperties( - common::Composite* comp, const StateData& state = StateData(), const PropertiesData& properties = PropertiesData()) - : AspectPropertiesImpl(comp, properties, state) + : AspectPropertiesImpl(properties, state) { // Do nothing } /// Construct using a PropertiesData and a StateData instance AspectWithStateAndVersionedProperties( - common::Composite* comp, const PropertiesData& properties, const StateData& state = StateData()) - : AspectPropertiesImpl(comp, properties, state) + : AspectPropertiesImpl(properties, state) { // Do nothing } // Documentation inherited - std::unique_ptr cloneAspect(Composite* newComposite) const override + std::unique_ptr cloneAspect() const override { - return make_unique( - newComposite, this->getState(), this->getProperties()); + return make_unique(this->getState(), this->getProperties()); } }; diff --git a/dart/common/Cloneable.hpp b/dart/common/Cloneable.hpp index 08df0a85a3f42..15be9ee24d941 100644 --- a/dart/common/Cloneable.hpp +++ b/dart/common/Cloneable.hpp @@ -248,11 +248,27 @@ class CloneableMap /// Map-based move assignment operator CloneableMap& operator=(MapType&& otherMap); - /// Copy the contents of another cloneable map into this one. - void copy(const CloneableMap& otherMap); - - /// Copy the contents of a map into this one. - void copy(const MapType& otherMap); + /// Copy the contents of another cloneable map into this one. If merge is set + /// to false, then any fields in this map which are not present in the other + /// map will be erased; otherwise they will be kept. Setting merge to false + /// will make the contents of this map an exact duplicate of the other map. + void copy(const CloneableMap& otherMap, bool merge=false); + + /// Copy the contents of a map into this one. If merge is set to false, then + /// any fields in this map which are not present in the other map will be + /// erased; otherwise they will be kept. Setting merge to false will make the + /// contents of this map an exact duplicate of the other map. + void copy(const MapType& otherMap, bool merge=false); + + /// Merge the contents of another cloneable map into this one. If there are + /// any entries which both maps have, then the contents of otherMap will take + /// precedence. This is the same as calling copy(otherMap, true). + void merge(const CloneableMap& otherMap); + + /// Merge the contents of another map into this one. If there are any entries + /// which both maps have, then the contents of otherMap will take precedence. + /// This is the same as calling copy(otherMap, true). + void merge(const MapType& otherMap); /// Get the map that is being held MapType& getMap(); diff --git a/dart/common/Composite.cpp b/dart/common/Composite.cpp index b70495cb3a1bf..38fe4eb6992ae 100644 --- a/dart/common/Composite.cpp +++ b/dart/common/Composite.cpp @@ -283,7 +283,7 @@ void Composite::_set(std::type_index type_idx, const Aspect* aspect) { if(aspect) { - mAspectMap[type_idx] = aspect->cloneAspect(this); + mAspectMap[type_idx] = aspect->cloneAspect(); addToComposite(mAspectMap[type_idx].get()); } else diff --git a/dart/common/EmbeddedAspect.hpp b/dart/common/EmbeddedAspect.hpp index b957584f29a46..41bb08b6a698d 100644 --- a/dart/common/EmbeddedAspect.hpp +++ b/dart/common/EmbeddedAspect.hpp @@ -316,63 +316,56 @@ class EmbeddedStateAndPropertiesAspect : using CompositeType = CompositeT; - EmbeddedStateAndPropertiesAspect() = delete; EmbeddedStateAndPropertiesAspect( const EmbeddedStateAndPropertiesAspect&) = delete; virtual ~EmbeddedStateAndPropertiesAspect() = default; /// Construct using nothing. The object will remain unaffected. - EmbeddedStateAndPropertiesAspect( - common::Composite* comp) - : AspectPropertiesImpl(comp) + EmbeddedStateAndPropertiesAspect() + : AspectPropertiesImpl() { // Do nothing } /// Construct using a State. The object's Properties will remain unaffected. EmbeddedStateAndPropertiesAspect( - common::Composite* comp, const StateData& state) - : AspectPropertiesImpl(comp, state) + : AspectPropertiesImpl(state) { // Do nothing } /// Construct using Properties. The object's State will remain unaffected. EmbeddedStateAndPropertiesAspect( - common::Composite* comp, const PropertiesData& properties) - : AspectPropertiesImpl(comp, properties) + : AspectPropertiesImpl(properties) { // Do nothing } /// Construct using a State and Properties instance EmbeddedStateAndPropertiesAspect( - common::Composite* comp, const StateData& state, const PropertiesData& properties) - : AspectPropertiesImpl(comp, properties, state) + : AspectPropertiesImpl(properties, state) { // Do nothing } /// Construct using a Properties and State instance EmbeddedStateAndPropertiesAspect( - common::Composite* comp, const PropertiesData& properties, const StateData& state) - : AspectPropertiesImpl(comp, properties, state) + : AspectPropertiesImpl(properties, state) { // Do nothing } // Documentation inherited - std::unique_ptr cloneAspect(Composite* newComposite) const override + std::unique_ptr cloneAspect() const override { - return make_unique( - newComposite, this->getState(), this->getProperties()); + return make_unique(this->getState(), this->getProperties()); } }; diff --git a/dart/common/ProxyAspect.hpp b/dart/common/ProxyAspect.hpp index 576f0e01d862f..0681dfe38644b 100644 --- a/dart/common/ProxyAspect.hpp +++ b/dart/common/ProxyAspect.hpp @@ -82,9 +82,9 @@ class ProxyStateAndPropertiesAspect : } // Documentation inherited - std::unique_ptr cloneAspect(Composite* newComposite) const override + std::unique_ptr cloneAspect() const override { - return make_unique(newComposite); + return make_unique(); } }; diff --git a/dart/common/detail/Aspect.hpp b/dart/common/detail/Aspect.hpp index edaddfbbe4bfc..cc1a5ec24c1ed 100644 --- a/dart/common/detail/Aspect.hpp +++ b/dart/common/detail/Aspect.hpp @@ -47,9 +47,8 @@ namespace common { //============================================================================== template -CompositeTrackingAspect::CompositeTrackingAspect(Composite* comp) - : Aspect(comp), - mComposite(nullptr) // This will be set later when the Composite calls setComposite +CompositeTrackingAspect::CompositeTrackingAspect() + : mComposite(nullptr) // This will be set later when the Composite calls setComposite { // Do nothing } @@ -82,15 +81,9 @@ void CompositeTrackingAspect::setComposite(Composite* newComposit assert(nullptr == mComposite); mComposite = dynamic_cast(newComposite); - if(nullptr == mComposite) - { - dterr << "[" << typeid(*this).name() << "::setComposite] Attempting to use a " - << "[" << typeid(newComposite).name() << "] type composite, but this " - << "Aspect is only designed to be attached to a [" - << typeid(CompositeType).name() << "] type composite. This may cause " - << "undefined behavior!\n"; - assert(false); - } + // Note: Derived classes should be responsible for handling the case in which + // the new composite type does not match the expected type. We should not + // assume here that it is an error. } //============================================================================== diff --git a/dart/common/detail/AspectWithVersion.hpp b/dart/common/detail/AspectWithVersion.hpp index 4f1b61ef38f88..c34b1453d75d0 100644 --- a/dart/common/detail/AspectWithVersion.hpp +++ b/dart/common/detail/AspectWithVersion.hpp @@ -64,17 +64,16 @@ class AspectWithState : public BaseT using AspectImplementation = AspectWithState< Base, Derived, StateData, CompositeT, updateState>; - AspectWithState() = delete; AspectWithState(const AspectWithState&) = delete; /// Construct using a StateData instance - AspectWithState(Composite* comp, const StateData& state = StateData()); + AspectWithState(const StateData& state = StateData()); /// Construct this Aspect and pass args into the constructor of the Base class template - AspectWithState(Composite* comp, const StateData& state, + AspectWithState(const StateData& state, BaseArgs&&... args) - : Base(comp, std::forward(args)...), + : Base(std::forward(args)...), mState(state) { // Do nothing @@ -93,8 +92,7 @@ class AspectWithState : public BaseT const State& getState() const; // Documentation inherited - std::unique_ptr cloneAspect( - Composite* newComposite) const override; + std::unique_ptr cloneAspect() const override; protected: @@ -127,13 +125,13 @@ class AspectWithVersionedProperties : public BaseT /// Construct using a PropertiesData instance AspectWithVersionedProperties( - Composite* comp, const PropertiesData& properties = PropertiesData()); + const PropertiesData& properties = PropertiesData()); /// Construct this Aspect and pass args into the constructor of the Base class template AspectWithVersionedProperties( - Composite* comp, const PropertiesData& properties, BaseArgs&&... args) - : Base(comp, std::forward(args)...), + const PropertiesData& properties, BaseArgs&&... args) + : Base(std::forward(args)...), mProperties(properties) { // Do nothing @@ -152,8 +150,7 @@ class AspectWithVersionedProperties : public BaseT const Properties& getProperties() const; // Documentation inherited - std::unique_ptr cloneAspect( - Composite* newComposite) const override; + std::unique_ptr cloneAspect() const override; /// Increment the version of this Aspect and its Composite std::size_t incrementVersion(); @@ -185,8 +182,8 @@ constexpr void (*AspectWithState< template AspectWithState:: -AspectWithState(Composite* comp, const StateDataT& state) - : BaseT(comp), +AspectWithState(const StateDataT& state) + : BaseT(), mState(state) { // Do nothing @@ -235,9 +232,9 @@ template std::unique_ptr AspectWithState:: - cloneAspect(Composite* newComposite) const + cloneAspect() const { - return common::make_unique(newComposite, mState); + return common::make_unique(mState); } //============================================================================== @@ -258,9 +255,8 @@ template AspectWithVersionedProperties:: -AspectWithVersionedProperties( - Composite* comp, const PropertiesData& properties) - : BaseT(comp), +AspectWithVersionedProperties(const PropertiesData& properties) + : BaseT(), mProperties(properties) { // Do nothing @@ -314,9 +310,9 @@ template AspectWithVersionedProperties:: -cloneAspect(Composite* newComposite) const +cloneAspect() const { - return common::make_unique(newComposite, mProperties); + return common::make_unique(mProperties); } //============================================================================== diff --git a/dart/common/detail/Cloneable.hpp b/dart/common/detail/Cloneable.hpp index 80c9b4544c1c3..676e98efbb41e 100644 --- a/dart/common/detail/Cloneable.hpp +++ b/dart/common/detail/Cloneable.hpp @@ -426,14 +426,14 @@ CloneableMap& CloneableMap::operator=( //============================================================================== template -void CloneableMap::copy(const CloneableMap& otherMap) +void CloneableMap::copy(const CloneableMap& otherMap, bool merge) { - copy(otherMap.getMap()); + copy(otherMap.getMap(), merge); } //============================================================================== template -void CloneableMap::copy(const MapType& otherMap) +void CloneableMap::copy(const MapType& otherMap, bool merge) { typename MapType::iterator receiver = mMap.begin(); typename MapType::const_iterator sender = otherMap.begin(); @@ -449,36 +449,71 @@ void CloneableMap::copy(const MapType& otherMap) } else if( receiver->first == sender->first ) { - // We should copy the incoming object when possible so we can avoid the - // memory allocation overhead of cloning. - if(receiver->second) - receiver->second->copy(*sender->second); - else - receiver->second = sender->second->clone(); + if(sender->second) + { + // If the sender has an object, we should copy it. + if(receiver->second) + // We should copy instead of cloning the incoming object when possible + // so we can avoid the memory allocation overhead of cloning. + receiver->second->copy(*sender->second); + else + receiver->second = sender->second->clone(); + } + else if(!merge) + { + // If the sender has no object, we should clear this one. + receiver->second = nullptr; + } ++receiver; ++sender; } else if( receiver->first < sender->first ) { - // Clear this entry in the map, because it does not have an analog in the - // map that we are copying - receiver->second = nullptr; + if(!merge) + { + // Clear this entry in the map, because it does not have an analog in + // the map that we are copying + receiver->second = nullptr; + } ++receiver; } else { - mMap[sender->first] = sender->second->clone(); + if(sender->second) + { + // If receiver has a higher value, then the receiving map does not + // contain an entry for this entry of the sending map, and therefore the + // entry must be created. + mMap[sender->first] = sender->second->clone(); + } ++sender; } } - while( mMap.end() != receiver ) + if(!merge) { - mMap.erase(receiver++); + while( mMap.end() != receiver ) + { + mMap.erase(receiver++); + } } } +//============================================================================== +template +void CloneableMap::merge(const CloneableMap& otherMap) +{ + copy(otherMap, true); +} + +//============================================================================== +template +void CloneableMap::merge(const MapType& otherMap) +{ + copy(otherMap, true); +} + //============================================================================== template MapType& CloneableMap::getMap() diff --git a/dart/common/detail/Composite.hpp b/dart/common/detail/Composite.hpp index b96ef66ec1163..24e8716f6c8a4 100644 --- a/dart/common/detail/Composite.hpp +++ b/dart/common/detail/Composite.hpp @@ -94,7 +94,7 @@ void Composite::set(std::unique_ptr&& aspect) template T* Composite::createAspect(Args&&... args) { - T* aspect = new T(this, std::forward(args)...); + T* aspect = new T(std::forward(args)...); mAspectMap[typeid(T)] = std::unique_ptr(aspect); addToComposite(aspect); @@ -207,7 +207,7 @@ void createAspects(T* comp) return this->template createAspect(std::forward(args)...);\ }\ \ - inline void erase ## AspectName ()\ + inline void remove ## AspectName ()\ {\ this->template removeAspect();\ }\ diff --git a/dart/common/detail/CompositeData.hpp b/dart/common/detail/CompositeData.hpp index 7ed2f93c97229..62e596adbe374 100644 --- a/dart/common/detail/CompositeData.hpp +++ b/dart/common/detail/CompositeData.hpp @@ -142,6 +142,11 @@ class CompositeData : public CloneableMap return static_cast(*it.first); } + template + bool has() const + { + return (get() != nullptr); + } }; //============================================================================== diff --git a/dart/common/detail/EmbeddedAspect.hpp b/dart/common/detail/EmbeddedAspect.hpp index 5f9bc2693aa99..997d84790dde4 100644 --- a/dart/common/detail/EmbeddedAspect.hpp +++ b/dart/common/detail/EmbeddedAspect.hpp @@ -92,7 +92,6 @@ class EmbeddedStateAspect : public BaseT enum DelegateTag { Delegate }; - EmbeddedStateAspect() = delete; EmbeddedStateAspect(const EmbeddedStateAspect&) = delete; virtual ~EmbeddedStateAspect() = default; @@ -107,8 +106,8 @@ class EmbeddedStateAspect : public BaseT }; /// Construct this Aspect without affecting the State. - EmbeddedStateAspect(Composite* comp) - : Base(comp) + EmbeddedStateAspect() + : Base() { // Do nothing } @@ -130,10 +129,10 @@ class EmbeddedStateAspect : public BaseT // these constraints, I would gladly replace this implementation. -(MXG) template EmbeddedStateAspect( - Composite* comp, const T& arg1, + const T& arg1, RemainingArgs&&... remainingArgs) : EmbeddedStateAspect( - Delegate, comp, + Delegate, static_cast::type&>(arg1), std::forward(remainingArgs)...) { @@ -187,9 +186,9 @@ class EmbeddedStateAspect : public BaseT } // Documentation inherited - std::unique_ptr cloneAspect(Composite* newComposite) const override + std::unique_ptr cloneAspect() const override { - return make_unique(newComposite, this->getState()); + return make_unique(this->getState()); } protected: @@ -198,9 +197,9 @@ class EmbeddedStateAspect : public BaseT /// arguments into the constructor of the Base class. template EmbeddedStateAspect( - DelegateTag, Composite* comp, const StateData& state, + DelegateTag, const StateData& state, RemainingArgs&&... remainingArgs) - : Base(comp, std::forward(remainingArgs)...), + : Base(std::forward(remainingArgs)...), mTemporaryState(make_unique(state)) { // Do nothing @@ -210,8 +209,8 @@ class EmbeddedStateAspect : public BaseT /// arguments into the constructor of the Base class. template EmbeddedStateAspect( - DelegateTag, Composite* comp, BaseArgs&&... args) - : Base(comp, std::forward(args)...) + DelegateTag, BaseArgs&&... args) + : Base(std::forward(args)...) { // Do nothing } @@ -266,7 +265,6 @@ class EmbeddedPropertiesAspect : public BaseT constexpr static void (*SetEmbeddedProperties)(Derived*, const Properties&) = setEmbeddedProperties; constexpr static const Properties& (*GetEmbeddedProperties)(const Derived*) = getEmbeddedProperties; - EmbeddedPropertiesAspect() = delete; EmbeddedPropertiesAspect(const EmbeddedPropertiesAspect&) = delete; virtual ~EmbeddedPropertiesAspect() = default; @@ -281,8 +279,8 @@ class EmbeddedPropertiesAspect : public BaseT }; /// Construct this Aspect without affecting the Properties. - EmbeddedPropertiesAspect(Composite* comp) - : Base(comp) + EmbeddedPropertiesAspect() + : Base() { // Do nothing } @@ -304,10 +302,10 @@ class EmbeddedPropertiesAspect : public BaseT // these constraints, I would gladly replace this implementation. -(MXG) template EmbeddedPropertiesAspect( - Composite* comp, const T& arg1, + const T& arg1, RemainingArgs&&... remainingArgs) : EmbeddedPropertiesAspect( - Delegate, comp, + Delegate, static_cast::type&>(arg1), std::forward(remainingArgs)...) { @@ -360,9 +358,9 @@ class EmbeddedPropertiesAspect : public BaseT return *mTemporaryProperties; } - std::unique_ptr cloneAspect(Composite* newComposite) const override + std::unique_ptr cloneAspect() const override { - return make_unique(newComposite, this->getProperties()); + return make_unique(this->getProperties()); } protected: @@ -371,9 +369,9 @@ class EmbeddedPropertiesAspect : public BaseT /// arguments into the constructor of the Base class. template EmbeddedPropertiesAspect( - DelegateTag, Composite* comp, const PropertiesData& properties, + DelegateTag, const PropertiesData& properties, RemainingArgs&&... remainingArgs) - : Base(comp, std::forward(remainingArgs)...), + : Base(std::forward(remainingArgs)...), mTemporaryProperties(make_unique(properties)) { // Do nothing @@ -383,8 +381,8 @@ class EmbeddedPropertiesAspect : public BaseT /// arguments into the constructor of the Base class. template EmbeddedPropertiesAspect( - DelegateTag, Composite* comp, BaseArgs&&... args) - : Base(comp, std::forward(args)...) + DelegateTag, BaseArgs&&... args) + : Base(std::forward(args)...) { // Do nothing } diff --git a/dart/common/detail/ProxyAspect.hpp b/dart/common/detail/ProxyAspect.hpp index 4a04d4ffc4e7a..e90debddeff43 100644 --- a/dart/common/detail/ProxyAspect.hpp +++ b/dart/common/detail/ProxyAspect.hpp @@ -57,9 +57,9 @@ class ProxyStateAspect : public BaseT /// General constructor template - ProxyStateAspect(Composite* comp, Args&&... args) - : Base(comp, std::forward(args)...), - mProxyState(dynamic_cast(comp)) + ProxyStateAspect(Args&&... args) + : Base(std::forward(args)...), + mProxyState() { // Do nothing } @@ -77,9 +77,9 @@ class ProxyStateAspect : public BaseT } // Documentation inherited - std::unique_ptr cloneAspect(Composite* newComposite) const override + std::unique_ptr cloneAspect() const override { - return make_unique(newComposite); + return make_unique(); } protected: @@ -126,9 +126,9 @@ class ProxyPropertiesAspect : public BaseT /// General constructor template - ProxyPropertiesAspect(Composite* comp, Args&&... args) - : Base(comp, std::forward(args)...), - mProxyProperties(dynamic_cast(this)) + ProxyPropertiesAspect(Args&&... args) + : Base(std::forward(args)...), + mProxyProperties() { // Do nothing } @@ -146,9 +146,9 @@ class ProxyPropertiesAspect : public BaseT } // Documentation inherited - std::unique_ptr cloneAspect(Composite* newComposite) const override + std::unique_ptr cloneAspect() const override { - return make_unique(newComposite); + return make_unique(); } protected: diff --git a/dart/common/detail/SpecializedForAspect.hpp b/dart/common/detail/SpecializedForAspect.hpp index 87f3560b744a2..1a5bdb48c57c2 100644 --- a/dart/common/detail/SpecializedForAspect.hpp +++ b/dart/common/detail/SpecializedForAspect.hpp @@ -210,7 +210,7 @@ void SpecializedForAspect::_set( if(aspect) { - mSpecAspectIterator->second = aspect->cloneAspect(this); + mSpecAspectIterator->second = aspect->cloneAspect(); addToComposite(mSpecAspectIterator->second.get()); } else @@ -258,7 +258,7 @@ SpecAspect* SpecializedForAspect::_createAspect( usedSpecializedAspectAccess = true; #endif // DART_UNITTEST_SPECIALIZED_ASPECT_ACCESS - SpecAspect* aspect = new SpecAspect(this, std::forward(args)...); + SpecAspect* aspect = new SpecAspect(std::forward(args)...); mSpecAspectIterator->second = std::unique_ptr(aspect); addToComposite(aspect); diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp index 74bf96b2e2517..5a1b904212eb8 100644 --- a/dart/constraint/ConstraintSolver.cpp +++ b/dart/constraint/ConstraintSolver.cpp @@ -69,7 +69,7 @@ ConstraintSolver::ConstraintSolver(double timeStep) mCollisionGroup(mCollisionDetector->createCollisionGroupAsSharedPtr()), mCollisionOption( collision::CollisionOption( - true, false, 100, std::make_shared())), + true, 1000u, std::make_shared())), mTimeStep(timeStep), mLCPSolver(new DantzigLCPSolver(mTimeStep)) { @@ -96,20 +96,18 @@ void ConstraintSolver::addSkeleton(const SkeletonPtr& skeleton) assert(skeleton && "Null pointer skeleton is now allowed to add to ConstraintSover."); - if (!containSkeleton(skeleton)) - { - auto group = mCollisionDetector->createCollisionGroup(skeleton.get()); - mCollisionGroup->addShapeFramesOf(group.get()); - - mSkeletons.push_back(skeleton); - mConstrainedGroups.reserve(mSkeletons.size()); - } - else + if (containSkeleton(skeleton)) { dtwarn << "[ConstraintSolver::addSkeleton] Attempting to add " << "skeleton '" << skeleton->getName() << "', which already exists in the ConstraintSolver.\n"; + + return; } + + mCollisionGroup->addShapeFramesOf(skeleton.get()); + mSkeletons.push_back(skeleton); + mConstrainedGroups.reserve(mSkeletons.size()); } //============================================================================== @@ -125,21 +123,17 @@ void ConstraintSolver::removeSkeleton(const SkeletonPtr& skeleton) assert(skeleton && "Null pointer skeleton is now allowed to add to ConstraintSover."); - if (containSkeleton(skeleton)) - { - auto group = mCollisionDetector->createCollisionGroup(skeleton.get()); - mCollisionGroup->removeShapeFramesOf(group.get()); - - mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), skeleton), - mSkeletons.end()); - mConstrainedGroups.reserve(mSkeletons.size()); - } - else + if (!containSkeleton(skeleton)) { dtwarn << "[ConstraintSolver::removeSkeleton] Attempting to remove " << "skeleton '" << skeleton->getName() << "', which doesn't exist in the ConstraintSolver.\n"; } + + mCollisionGroup->removeShapeFramesOf(skeleton.get()); + mSkeletons.erase(remove(mSkeletons.begin(), mSkeletons.end(), skeleton), + mSkeletons.end()); + mConstrainedGroups.reserve(mSkeletons.size()); } //============================================================================== @@ -393,7 +387,7 @@ void ConstraintSolver::updateConstraints() //---------------------------------------------------------------------------- mCollisionResult.clear(); - mCollisionGroup->collide(mCollisionOption, mCollisionResult); + mCollisionGroup->collide(mCollisionOption, &mCollisionResult); // Destroy previous contact constraints mContactConstraints.clear(); diff --git a/dart/dynamics/BallJoint.cpp b/dart/dynamics/BallJoint.cpp index e5006e9d28a5a..28e9055de4423 100644 --- a/dart/dynamics/BallJoint.cpp +++ b/dart/dynamics/BallJoint.cpp @@ -116,7 +116,7 @@ Joint* BallJoint::clone() const } //============================================================================== -Eigen::Matrix BallJoint::getLocalJacobianStatic( +Eigen::Matrix BallJoint::getRelativeJacobianStatic( const Eigen::Vector3d& /*positions*/) const { return mJacobian; @@ -153,7 +153,7 @@ void BallJoint::updateDegreeOfFreedomNames() } //============================================================================== -void BallJoint::updateLocalTransform() const +void BallJoint::updateRelativeTransform() const { mR.linear() = convertToRotation(getPositionsStatic()); @@ -164,14 +164,14 @@ void BallJoint::updateLocalTransform() const } //============================================================================== -void BallJoint::updateLocalJacobian(bool _mandatory) const +void BallJoint::updateRelativeJacobian(bool _mandatory) const { if (_mandatory) mJacobian = math::getAdTMatrix(Joint::mAspectProperties.mT_ChildBodyToJoint).leftCols<3>(); } //============================================================================== -void BallJoint::updateLocalJacobianTimeDeriv() const +void BallJoint::updateRelativeJacobianTimeDeriv() const { assert(Eigen::Matrix6d::Zero().leftCols<3>() == mJacobianDeriv); } @@ -181,7 +181,7 @@ const Eigen::Isometry3d& BallJoint::getR() const { if(mNeedTransformUpdate) { - updateLocalTransform(); + updateRelativeTransform(); mNeedTransformUpdate = false; } diff --git a/dart/dynamics/BallJoint.hpp b/dart/dynamics/BallJoint.hpp index 66d3e22245e91..7f67d46dd0442 100644 --- a/dart/dynamics/BallJoint.hpp +++ b/dart/dynamics/BallJoint.hpp @@ -92,7 +92,7 @@ class BallJoint : public MultiDofJoint<3> static Eigen::Matrix3d convertToRotation(const Eigen::Vector3d& _positions); // Documentation inherited - Eigen::Matrix getLocalJacobianStatic( + Eigen::Matrix getRelativeJacobianStatic( const Eigen::Vector3d& _positions) const override; // Documentation inherited @@ -107,7 +107,7 @@ class BallJoint : public MultiDofJoint<3> // Documentation inherited Joint* clone() const override; - using MultiDofJoint::getLocalJacobianStatic; + using MultiDofJoint::getRelativeJacobianStatic; // Documentation inherited void integratePositions(double _dt) override; @@ -116,13 +116,13 @@ class BallJoint : public MultiDofJoint<3> void updateDegreeOfFreedomNames() override; // Documentation inherited - void updateLocalTransform() const override; + void updateRelativeTransform() const override; // Documentation inherited - void updateLocalJacobian(bool _mandatory = true) const override; + void updateRelativeJacobian(bool _mandatory = true) const override; // Documentation inherited - void updateLocalJacobianTimeDeriv() const override; + void updateRelativeJacobianTimeDeriv() const override; protected: diff --git a/dart/dynamics/BodyNode.cpp b/dart/dynamics/BodyNode.cpp index 48a7aef98a6f1..d3edd332b59bd 100644 --- a/dart/dynamics/BodyNode.cpp +++ b/dart/dynamics/BodyNode.cpp @@ -1076,25 +1076,25 @@ const std::vector BodyNode::getChainDofs() const //============================================================================== const Eigen::Isometry3d& BodyNode::getRelativeTransform() const { - return mParentJoint->getLocalTransform(); + return mParentJoint->getRelativeTransform(); } //============================================================================== const Eigen::Vector6d& BodyNode::getRelativeSpatialVelocity() const { - return mParentJoint->getLocalSpatialVelocity(); + return mParentJoint->getRelativeSpatialVelocity(); } //============================================================================== const Eigen::Vector6d& BodyNode::getRelativeSpatialAcceleration() const { - return mParentJoint->getLocalSpatialAcceleration(); + return mParentJoint->getRelativeSpatialAcceleration(); } //============================================================================== const Eigen::Vector6d& BodyNode::getPrimaryRelativeAcceleration() const { - return mParentJoint->getLocalPrimaryAcceleration(); + return mParentJoint->getRelativePrimaryAcceleration(); } //============================================================================== @@ -1610,7 +1610,7 @@ void BodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, Joint* childJoint = childBodyNode->getParentJoint(); assert(childJoint != nullptr); - mF += math::dAdInvT(childJoint->getLocalTransform(), + mF += math::dAdInvT(childJoint->getRelativeTransform(), childBodyNode->getBodyForce()); } @@ -1759,7 +1759,7 @@ void BodyNode::updateVelocityChangeFD() mParentBodyNode->mDelV); // Transmit spatial acceleration of parent body to this body - mDelV = math::AdInvT(mParentJoint->getLocalTransform(), + mDelV = math::AdInvT(mParentJoint->getRelativeTransform(), mParentBodyNode->mDelV); } else @@ -1983,14 +1983,14 @@ void BodyNode::aggregateGravityForceVector(Eigen::VectorXd& _g, for (std::vector::const_iterator it = mChildBodyNodes.begin(); it != mChildBodyNodes.end(); ++it) { - mG_F += math::dAdInvT((*it)->mParentJoint->getLocalTransform(), + mG_F += math::dAdInvT((*it)->mParentJoint->getRelativeTransform(), (*it)->mG_F); } std::size_t nGenCoords = mParentJoint->getNumDofs(); if (nGenCoords > 0) { - Eigen::VectorXd g = -(mParentJoint->getLocalJacobian().transpose() * mG_F); + Eigen::VectorXd g = -(mParentJoint->getRelativeJacobian().transpose() * mG_F); std::size_t iStart = mParentJoint->getIndexInTree(0); _g.segment(iStart, nGenCoords) = g; } @@ -2001,7 +2001,7 @@ void BodyNode::updateCombinedVector() { if (mParentBodyNode) { - mCg_dV = math::AdInvT(mParentJoint->getLocalTransform(), + mCg_dV = math::AdInvT(mParentJoint->getRelativeTransform(), mParentBodyNode->mCg_dV) + getPartialAcceleration(); } else @@ -2037,7 +2037,7 @@ void BodyNode::aggregateCombinedVector(Eigen::VectorXd& _Cg, if (nGenCoords > 0) { Eigen::VectorXd Cg - = mParentJoint->getLocalJacobian().transpose() * mCg_F; + = mParentJoint->getRelativeJacobian().transpose() * mCg_F; std::size_t iStart = mParentJoint->getIndexInTree(0); _Cg.segment(iStart, nGenCoords) = Cg; } @@ -2051,14 +2051,14 @@ void BodyNode::aggregateExternalForces(Eigen::VectorXd& _Fext) for (std::vector::const_iterator it = mChildBodyNodes.begin(); it != mChildBodyNodes.end(); ++it) { - mFext_F += math::dAdInvT((*it)->mParentJoint->getLocalTransform(), + mFext_F += math::dAdInvT((*it)->mParentJoint->getRelativeTransform(), (*it)->mFext_F); } std::size_t nGenCoords = mParentJoint->getNumDofs(); if (nGenCoords > 0) { - Eigen::VectorXd Fext = mParentJoint->getLocalJacobian().transpose()*mFext_F; + Eigen::VectorXd Fext = mParentJoint->getRelativeJacobian().transpose()*mFext_F; std::size_t iStart = mParentJoint->getIndexInTree(0); _Fext.segment(iStart, nGenCoords) = Fext; } @@ -2075,7 +2075,7 @@ void BodyNode::aggregateSpatialToGeneralized(Eigen::VectorXd& _generalized, for (std::vector::const_iterator it = mChildBodyNodes.begin(); it != mChildBodyNodes.end(); ++it) { - mArbitrarySpatial += math::dAdInvT((*it)->mParentJoint->getLocalTransform(), + mArbitrarySpatial += math::dAdInvT((*it)->mParentJoint->getRelativeTransform(), (*it)->mArbitrarySpatial); } @@ -2092,12 +2092,12 @@ void BodyNode::updateMassMatrix() std::size_t dof = mParentJoint->getNumDofs(); if (dof > 0) { - mM_dV.noalias() += mParentJoint->getLocalJacobian() + mM_dV.noalias() += mParentJoint->getRelativeJacobian() * mParentJoint->getAccelerations(); assert(!math::isNan(mM_dV)); } if (mParentBodyNode) - mM_dV += math::AdInvT(mParentJoint->getLocalTransform(), + mM_dV += math::AdInvT(mParentJoint->getRelativeTransform(), mParentBodyNode->mM_dV); assert(!math::isNan(mM_dV)); } @@ -2116,7 +2116,7 @@ void BodyNode::aggregateMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col) for (std::vector::const_iterator it = mChildBodyNodes.begin(); it != mChildBodyNodes.end(); ++it) { - mM_F += math::dAdInvT((*it)->getParentJoint()->getLocalTransform(), + mM_F += math::dAdInvT((*it)->getParentJoint()->getRelativeTransform(), (*it)->mM_F); } @@ -2129,7 +2129,7 @@ void BodyNode::aggregateMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col) { std::size_t iStart = mParentJoint->getIndexInTree(0); _MCol.block(iStart, _col, dof, 1).noalias() = - mParentJoint->getLocalJacobian().transpose() * mM_F; + mParentJoint->getRelativeJacobian().transpose() * mM_F; } } @@ -2150,7 +2150,7 @@ void BodyNode::aggregateAugMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col, for (std::vector::const_iterator it = mChildBodyNodes.begin(); it != mChildBodyNodes.end(); ++it) { - mM_F += math::dAdInvT((*it)->getParentJoint()->getLocalTransform(), + mM_F += math::dAdInvT((*it)->getParentJoint()->getRelativeTransform(), (*it)->mM_F); } @@ -2172,7 +2172,7 @@ void BodyNode::aggregateAugMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col, std::size_t iStart = mParentJoint->getIndexInTree(0); _MCol.block(iStart, _col, dof, 1).noalias() - = mParentJoint->getLocalJacobian().transpose() * mM_F + = mParentJoint->getRelativeJacobian().transpose() * mM_F + D * (_timeStep * mParentJoint->getAccelerations()) + K * (_timeStep * _timeStep * mParentJoint->getAccelerations()); } @@ -2230,7 +2230,7 @@ void BodyNode::aggregateInvMassMatrix(Eigen::MatrixXd& _InvMCol, std::size_t _co _InvMCol, _col, getArticulatedInertia(), mParentBodyNode->mInvM_U); // - mInvM_U = math::AdInvT(mParentJoint->getLocalTransform(), + mInvM_U = math::AdInvT(mParentJoint->getRelativeTransform(), mParentBodyNode->mInvM_U); } else @@ -2259,7 +2259,7 @@ void BodyNode::aggregateInvAugMassMatrix(Eigen::MatrixXd& _InvMCol, std::size_t mParentBodyNode->mInvM_U); // - mInvM_U = math::AdInvT(mParentJoint->getLocalTransform(), + mInvM_U = math::AdInvT(mParentJoint->getRelativeTransform(), mParentBodyNode->mInvM_U); } else @@ -2308,12 +2308,12 @@ void BodyNode::updateBodyJacobian() const assert(mParentJoint); mBodyJacobian.leftCols(ascendantDof) = - math::AdInvTJac(mParentJoint->getLocalTransform(), + math::AdInvTJac(mParentJoint->getRelativeTransform(), mParentBodyNode->getJacobian()); } // Local Jacobian - mBodyJacobian.rightCols(localDof) = mParentJoint->getLocalJacobian(); + mBodyJacobian.rightCols(localDof) = mParentJoint->getRelativeJacobian(); mIsBodyJacobianDirty = false; } @@ -2360,13 +2360,13 @@ void BodyNode::updateBodyJacobianSpatialDeriv() const == static_cast(mBodyJacobianSpatialDeriv.cols())); mBodyJacobianSpatialDeriv.leftCols(numParentDOFs) - = math::AdInvTJac(mParentJoint->getLocalTransform(), dJ_parent); + = math::AdInvTJac(mParentJoint->getRelativeTransform(), dJ_parent); } // Local Jacobian: ad(V(i), S(i)) + dS(i) mBodyJacobianSpatialDeriv.rightCols(numLocalDOFs) - = math::adJac(getSpatialVelocity(), mParentJoint->getLocalJacobian()) - + mParentJoint->getLocalJacobianTimeDeriv(); + = math::adJac(getSpatialVelocity(), mParentJoint->getRelativeJacobian()) + + mParentJoint->getRelativeJacobianTimeDeriv(); mIsBodyJacobianSpatialDerivDirty = false; } @@ -2424,8 +2424,8 @@ void BodyNode::updateWorldJacobianClassicDeriv() const + dJ_parent.topRows<3>().colwise().cross(p); } - const math::Jacobian& dJ_local = mParentJoint->getLocalJacobianTimeDeriv(); - const math::Jacobian& J_local = mParentJoint->getLocalJacobian(); + const math::Jacobian& dJ_local = mParentJoint->getRelativeJacobianTimeDeriv(); + const math::Jacobian& J_local = mParentJoint->getRelativeJacobian(); const Eigen::Isometry3d& T = getWorldTransform(); const Eigen::Vector3d& w = getAngularVelocity(); diff --git a/dart/dynamics/CMakeLists.txt b/dart/dynamics/CMakeLists.txt index 04aeef8c25aa3..f1d1b0e1b085a 100644 --- a/dart/dynamics/CMakeLists.txt +++ b/dart/dynamics/CMakeLists.txt @@ -28,7 +28,7 @@ install( ) install( - FILES ${detail} + FILES ${detail_hdrs} DESTINATION include/dart/dynamics/detail COMPONENT headers ) diff --git a/dart/dynamics/EulerJoint.cpp b/dart/dynamics/EulerJoint.cpp index 324da0ef2a4b7..267f95ab8f7f1 100644 --- a/dart/dynamics/EulerJoint.cpp +++ b/dart/dynamics/EulerJoint.cpp @@ -129,7 +129,7 @@ void EulerJoint::setAxisOrder(EulerJoint::AxisOrder _order, bool _renameDofs) updateDegreeOfFreedomNames(); Joint::notifyPositionUpdate(); - updateLocalJacobian(true); + updateRelativeJacobian(true); Joint::incrementVersion(); } @@ -180,7 +180,7 @@ Eigen::Matrix3d EulerJoint::convertToRotation(const Eigen::Vector3d& _positions) } //============================================================================== -Eigen::Matrix EulerJoint::getLocalJacobianStatic( +Eigen::Matrix EulerJoint::getRelativeJacobianStatic( const Eigen::Vector3d& _positions) const { Eigen::Matrix J; @@ -336,7 +336,7 @@ void EulerJoint::updateDegreeOfFreedomNames() } //============================================================================== -void EulerJoint::updateLocalTransform() const +void EulerJoint::updateRelativeTransform() const { mT = Joint::mAspectProperties.mT_ParentBodyToJoint * convertToTransform(getPositionsStatic()) * Joint::mAspectProperties.mT_ChildBodyToJoint.inverse(); @@ -345,13 +345,13 @@ void EulerJoint::updateLocalTransform() const } //============================================================================== -void EulerJoint::updateLocalJacobian(bool) const +void EulerJoint::updateRelativeJacobian(bool) const { - mJacobian = getLocalJacobianStatic(getPositionsStatic()); + mJacobian = getRelativeJacobianStatic(getPositionsStatic()); } //============================================================================== -void EulerJoint::updateLocalJacobianTimeDeriv() const +void EulerJoint::updateRelativeJacobianTimeDeriv() const { // double q0 = mPositions[0]; const Eigen::Vector3d& positions = getPositionsStatic(); diff --git a/dart/dynamics/EulerJoint.hpp b/dart/dynamics/EulerJoint.hpp index f30ef1be69ba4..11c5dc0cc3db4 100644 --- a/dart/dynamics/EulerJoint.hpp +++ b/dart/dynamics/EulerJoint.hpp @@ -147,7 +147,7 @@ class EulerJoint : public detail::EulerJointBase Eigen::Matrix3d convertToRotation(const Eigen::Vector3d& _positions) const; // Documentation inherited - Eigen::Matrix getLocalJacobianStatic( + Eigen::Matrix getRelativeJacobianStatic( const Eigen::Vector3d& _positions) const override; protected: @@ -158,20 +158,20 @@ class EulerJoint : public detail::EulerJointBase // Documentation inherited Joint* clone() const override; - using MultiDofJoint::getLocalJacobianStatic; + using MultiDofJoint::getRelativeJacobianStatic; /// Set the names of this joint's DegreesOfFreedom. Used during construction /// and when axis order is changed. void updateDegreeOfFreedomNames() override; // Documentation inherited - void updateLocalTransform() const override; + void updateRelativeTransform() const override; // Documentation inherited - void updateLocalJacobian(bool =true) const override; + void updateRelativeJacobian(bool =true) const override; // Documentation inherited - void updateLocalJacobianTimeDeriv() const override; + void updateRelativeJacobianTimeDeriv() const override; public: // To get byte-aligned Eigen vectors diff --git a/dart/dynamics/FreeJoint.cpp b/dart/dynamics/FreeJoint.cpp index 8e4c04d9e14e1..b3516d424c26e 100644 --- a/dart/dynamics/FreeJoint.cpp +++ b/dart/dynamics/FreeJoint.cpp @@ -187,7 +187,7 @@ void FreeJoint::setTransform(const Eigen::Isometry3d& newTransform, void FreeJoint::setRelativeSpatialVelocity( const Eigen::Vector6d& newSpatialVelocity) { - setVelocitiesStatic(getLocalJacobianStatic().inverse() * newSpatialVelocity); + setVelocitiesStatic(getRelativeJacobianStatic().inverse() * newSpatialVelocity); } //============================================================================== @@ -241,7 +241,7 @@ void FreeJoint::setSpatialVelocity(const Eigen::Vector6d& newSpatialVelocity, if (relativeTo->isWorld()) { const Eigen::Vector6d parentVelocity = math::AdInvT( - getLocalTransform(), + getRelativeTransform(), getChildBodyNode()->getParentFrame()->getSpatialVelocity()); targetRelSpatialVel -= parentVelocity; @@ -249,7 +249,7 @@ void FreeJoint::setSpatialVelocity(const Eigen::Vector6d& newSpatialVelocity, else { const Eigen::Vector6d parentVelocity = math::AdInvT( - getLocalTransform(), + getRelativeTransform(), getChildBodyNode()->getParentFrame()->getSpatialVelocity()); const Eigen::Vector6d arbitraryVelocity = math::AdT( relativeTo->getTransform(getChildBodyNode()), @@ -338,8 +338,8 @@ void FreeJoint::setAngularVelocity(const Eigen::Vector3d& newAngularVelocity, void FreeJoint::setRelativeSpatialAcceleration( const Eigen::Vector6d& newSpatialAcceleration) { - const Eigen::Matrix6d& J = getLocalJacobianStatic(); - const Eigen::Matrix6d& dJ = getLocalJacobianTimeDerivStatic(); + const Eigen::Matrix6d& J = getRelativeJacobianStatic(); + const Eigen::Matrix6d& dJ = getRelativeJacobianTimeDerivStatic(); setAccelerationsStatic( J.inverse() * (newSpatialAcceleration - dJ * getVelocitiesStatic())); @@ -399,10 +399,10 @@ void FreeJoint::setSpatialAcceleration( { const Eigen::Vector6d parentAcceleration = math::AdInvT( - getLocalTransform(), + getRelativeTransform(), getChildBodyNode()->getParentFrame()->getSpatialAcceleration()) + math::ad(getChildBodyNode()->getSpatialVelocity(), - getLocalJacobianStatic() * getVelocitiesStatic()); + getRelativeJacobianStatic() * getVelocitiesStatic()); targetRelSpatialAcc -= parentAcceleration; } @@ -410,10 +410,10 @@ void FreeJoint::setSpatialAcceleration( { const Eigen::Vector6d parentAcceleration = math::AdInvT( - getLocalTransform(), + getRelativeTransform(), getChildBodyNode()->getParentFrame()->getSpatialAcceleration()) + math::ad(getChildBodyNode()->getSpatialVelocity(), - getLocalJacobianStatic() * getVelocitiesStatic()); + getRelativeJacobianStatic() * getVelocitiesStatic()); const Eigen::Vector6d arbitraryAcceleration = math::AdT(relativeTo->getTransform(getChildBodyNode()), relativeTo->getSpatialAcceleration()) @@ -507,7 +507,7 @@ void FreeJoint::setAngularAcceleration( } //============================================================================== -Eigen::Matrix6d FreeJoint::getLocalJacobianStatic( +Eigen::Matrix6d FreeJoint::getRelativeJacobianStatic( const Eigen::Vector6d& /*positions*/) const { return mJacobian; @@ -590,7 +590,7 @@ void FreeJoint::updateDegreeOfFreedomNames() } //============================================================================== -void FreeJoint::updateLocalTransform() const +void FreeJoint::updateRelativeTransform() const { mQ = convertToTransform(getPositionsStatic()); @@ -601,14 +601,14 @@ void FreeJoint::updateLocalTransform() const } //============================================================================== -void FreeJoint::updateLocalJacobian(bool _mandatory) const +void FreeJoint::updateRelativeJacobian(bool _mandatory) const { if (_mandatory) mJacobian = math::getAdTMatrix(Joint::mAspectProperties.mT_ChildBodyToJoint); } //============================================================================== -void FreeJoint::updateLocalJacobianTimeDeriv() const +void FreeJoint::updateRelativeJacobianTimeDeriv() const { assert(Eigen::Matrix6d::Zero() == mJacobianDeriv); } @@ -618,7 +618,7 @@ const Eigen::Isometry3d& FreeJoint::getQ() const { if(mNeedTransformUpdate) { - updateLocalTransform(); + updateRelativeTransform(); mNeedTransformUpdate = false; } diff --git a/dart/dynamics/FreeJoint.hpp b/dart/dynamics/FreeJoint.hpp index 6d11c5796cb91..07c4d02fa94db 100644 --- a/dart/dynamics/FreeJoint.hpp +++ b/dart/dynamics/FreeJoint.hpp @@ -261,7 +261,7 @@ class FreeJoint : public MultiDofJoint<6> const Frame* inCoordinatesOf = Frame::World()); // Documentation inherited - Eigen::Matrix6d getLocalJacobianStatic( + Eigen::Matrix6d getRelativeJacobianStatic( const Eigen::Vector6d& _positions) const override; // Documentation inherited @@ -276,7 +276,7 @@ class FreeJoint : public MultiDofJoint<6> // Documentation inherited Joint* clone() const override; - using MultiDofJoint::getLocalJacobianStatic; + using MultiDofJoint::getRelativeJacobianStatic; // Documentation inherited void integratePositions(double _dt) override; @@ -285,13 +285,13 @@ class FreeJoint : public MultiDofJoint<6> void updateDegreeOfFreedomNames() override; // Documentation inherited - void updateLocalTransform() const override; + void updateRelativeTransform() const override; // Documentation inherited - void updateLocalJacobian(bool _mandatory = true) const override; + void updateRelativeJacobian(bool _mandatory = true) const override; // Documentation inherited - void updateLocalJacobianTimeDeriv() const override; + void updateRelativeJacobianTimeDeriv() const override; protected: diff --git a/dart/dynamics/Joint.cpp b/dart/dynamics/Joint.cpp index 305cdf8a20356..31c5083bebf06 100644 --- a/dart/dynamics/Joint.cpp +++ b/dart/dynamics/Joint.cpp @@ -267,10 +267,52 @@ std::shared_ptr Joint::getSkeleton() const //============================================================================== const Eigen::Isometry3d& Joint::getLocalTransform() const +{ + return getRelativeTransform(); +} + +//============================================================================== +const Eigen::Vector6d& Joint::getLocalSpatialVelocity() const +{ + return getRelativeSpatialVelocity(); +} + +//============================================================================== +const Eigen::Vector6d& Joint::getLocalSpatialAcceleration() const +{ + return getRelativeSpatialAcceleration(); +} + +//============================================================================== +const Eigen::Vector6d& Joint::getLocalPrimaryAcceleration() const +{ + return getRelativePrimaryAcceleration(); +} + +//============================================================================== +const math::Jacobian Joint::getLocalJacobian() const +{ + return getRelativeJacobian(); +} + +//============================================================================== +math::Jacobian Joint::getLocalJacobian(const Eigen::VectorXd& positions) const +{ + return getRelativeJacobian(positions); +} + +//============================================================================== +const math::Jacobian Joint::getLocalJacobianTimeDeriv() const +{ + return getRelativeJacobianTimeDeriv(); +} + +//============================================================================== +const Eigen::Isometry3d& Joint::getRelativeTransform() const { if(mNeedTransformUpdate) { - updateLocalTransform(); + updateRelativeTransform(); mNeedTransformUpdate = false; } @@ -278,11 +320,11 @@ const Eigen::Isometry3d& Joint::getLocalTransform() const } //============================================================================== -const Eigen::Vector6d& Joint::getLocalSpatialVelocity() const +const Eigen::Vector6d& Joint::getRelativeSpatialVelocity() const { if(mNeedSpatialVelocityUpdate) { - updateLocalSpatialVelocity(); + updateRelativeSpatialVelocity(); mNeedSpatialVelocityUpdate = false; } @@ -290,11 +332,11 @@ const Eigen::Vector6d& Joint::getLocalSpatialVelocity() const } //============================================================================== -const Eigen::Vector6d& Joint::getLocalSpatialAcceleration() const +const Eigen::Vector6d& Joint::getRelativeSpatialAcceleration() const { if(mNeedSpatialAccelerationUpdate) { - updateLocalSpatialAcceleration(); + updateRelativeSpatialAcceleration(); mNeedSpatialAccelerationUpdate = false; } @@ -302,11 +344,11 @@ const Eigen::Vector6d& Joint::getLocalSpatialAcceleration() const } //============================================================================== -const Eigen::Vector6d& Joint::getLocalPrimaryAcceleration() const +const Eigen::Vector6d& Joint::getRelativePrimaryAcceleration() const { if(mNeedPrimaryAccelerationUpdate) { - updateLocalPrimaryAcceleration(); + updateRelativePrimaryAcceleration(); mNeedPrimaryAccelerationUpdate = false; } @@ -406,7 +448,7 @@ void Joint::setTransformFromChildBodyNode(const Eigen::Isometry3d& _T) { assert(math::verifyTransform(_T)); mAspectProperties.mT_ChildBodyToJoint = _T; - updateLocalJacobian(); + updateRelativeJacobian(); notifyPositionUpdate(); } @@ -433,8 +475,8 @@ Joint::Joint() mNeedSpatialVelocityUpdate(true), mNeedSpatialAccelerationUpdate(true), mNeedPrimaryAccelerationUpdate(true), - mIsLocalJacobianDirty(true), - mIsLocalJacobianTimeDerivDirty(true) + mIsRelativeJacobianDirty(true), + mIsRelativeJacobianTimeDerivDirty(true) { // Do nothing. The Joint::Aspect must be created by a derived class. } @@ -445,6 +487,42 @@ DegreeOfFreedom* Joint::createDofPointer(std::size_t _indexInJoint) return new DegreeOfFreedom(this, _indexInJoint); } +//============================================================================== +void Joint::updateLocalTransform() const +{ + updateRelativeTransform(); +} + +//============================================================================== +void Joint::updateLocalSpatialVelocity() const +{ + updateRelativeSpatialVelocity(); +} + +//============================================================================== +void Joint::updateLocalSpatialAcceleration() const +{ + updateRelativeSpatialAcceleration(); +} + +//============================================================================== +void Joint::updateLocalPrimaryAcceleration() const +{ + updateRelativePrimaryAcceleration(); +} + +//============================================================================== +void Joint::updateLocalJacobian(bool mandatory) const +{ + updateRelativeJacobian(mandatory); +} + +//============================================================================== +void Joint::updateLocalJacobianTimeDeriv() const +{ + updateRelativeJacobianTimeDeriv(); +} + //============================================================================== void Joint::updateArticulatedInertia() const { @@ -489,8 +567,8 @@ void Joint::notifyPositionUpdate() mChildBodyNode->notifyJacobianDerivUpdate(); } - mIsLocalJacobianDirty = true; - mIsLocalJacobianTimeDerivDirty = true; + mIsRelativeJacobianDirty = true; + mIsRelativeJacobianTimeDerivDirty = true; mNeedPrimaryAccelerationUpdate = true; mNeedTransformUpdate = true; @@ -516,7 +594,7 @@ void Joint::notifyVelocityUpdate() mChildBodyNode->notifyJacobianDerivUpdate(); } - mIsLocalJacobianTimeDerivDirty = true; + mIsRelativeJacobianTimeDerivDirty = true; mNeedSpatialVelocityUpdate = true; mNeedSpatialAccelerationUpdate = true; diff --git a/dart/dynamics/Joint.hpp b/dart/dynamics/Joint.hpp index 3d4dcce87c0f8..0613e242b534e 100644 --- a/dart/dynamics/Joint.hpp +++ b/dart/dynamics/Joint.hpp @@ -42,6 +42,7 @@ #include #include +#include "dart/common/Deprecated.hpp" #include "dart/common/Subject.hpp" #include "dart/common/VersionCounter.hpp" #include "dart/common/EmbeddedAspect.hpp" @@ -288,7 +289,7 @@ class Joint : public virtual common::Subject, /// Get whether a generalized coordinate is cyclic. Return true if and only /// if this generalized coordinate has an infinite number of positions that - /// produce the same local transform. Note that, for a multi-DOF joint, + /// produce the same relative transform. Note that, for a multi-DOF joint, /// producing a cycle may require altering the position of this Joint's other /// generalized coordinates. virtual bool isCyclic(std::size_t _index) const = 0; @@ -543,39 +544,61 @@ class Joint : public virtual common::Subject, //---------------------------------------------------------------------------- - /// Get transformation from parent BodyNode to child BodyNode + /// Deprecated. Use getRelativeTransform() instead. + DEPRECATED(6.0) const Eigen::Isometry3d& getLocalTransform() const; - /// Get the velocity from the parent BodyNode to the child BodyNode + /// Deprecated. Use getLocalSpatialVelocity() instead. + DEPRECATED(6.0) const Eigen::Vector6d& getLocalSpatialVelocity() const; - /// Get the acceleration from the parent BodyNode to the child BodyNode + /// Deprecated. Use getLocalSpatialAcceleration() instead. + DEPRECATED(6.0) const Eigen::Vector6d& getLocalSpatialAcceleration() const; - /// Get the J * q_dd of this joint + /// Deprecated. Use getLocalPrimaryAcceleration() instead. + DEPRECATED(6.0) const Eigen::Vector6d& getLocalPrimaryAcceleration() const; - /// Get generalized Jacobian from parent body node to child body node - /// w.r.t. local generalized coordinate - virtual const math::Jacobian getLocalJacobian() const = 0; + /// Deprecated. Use getRelativeJacobian() instead. + DEPRECATED(6.0) + const math::Jacobian getLocalJacobian() const; - /// Get generalized Jacobian from parent body node to child body node - /// w.r.t. local generalized coordinate - virtual math::Jacobian getLocalJacobian( - const Eigen::VectorXd& _positions) const = 0; + /// Deprecated. Use getRelativeJacobian() instead. + DEPRECATED(6.0) + math::Jacobian getLocalJacobian(const Eigen::VectorXd& positions) const; - /// Get time derivative of generalized Jacobian from parent body node - /// to child body node w.r.t. local generalized coordinate - virtual const math::Jacobian getLocalJacobianTimeDeriv() const = 0; + /// Deprecated. Use getRelativeJacobianTimeDeriv() instead. + DEPRECATED(6.0) + const math::Jacobian getLocalJacobianTimeDeriv() const; - /// Get whether this joint contains _genCoord - /// \param[in] Generalized coordinate to see - /// \return True if this joint contains _genCoord -// bool contains(const GenCoord* _genCoord) const; + /// Get transform of the child BodyNode relative to the parent BodyNode + /// expressed in the child BodyNode frame + const Eigen::Isometry3d& getRelativeTransform() const; - /// Get local index of the dof at this joint; if the dof is not presented at - /// this joint, return -1 -// int getGenCoordLocalIndex(int _dofSkelIndex) const; + /// Get spatial velocity of the child BodyNode relative to the parent BodyNode + /// expressed in the child BodyNode frame + const Eigen::Vector6d& getRelativeSpatialVelocity() const; + + /// Get spatial acceleration of the child BodyNode relative to the parent + /// BodyNode expressed in the child BodyNode frame + const Eigen::Vector6d& getRelativeSpatialAcceleration() const; + + /// Get J * \ddot{q} of this joint + const Eigen::Vector6d& getRelativePrimaryAcceleration() const; + + /// Get spatial Jacobian of the child BodyNode relative to the parent BodyNode + /// expressed in the child BodyNode frame + virtual const math::Jacobian getRelativeJacobian() const = 0; + + /// Get spatial Jacobian of the child BodyNode relative to the parent BodyNode + /// expressed in the child BodyNode frame + virtual math::Jacobian getRelativeJacobian( + const Eigen::VectorXd& positions) const = 0; + + /// Get time derivative of spatial Jacobian of the child BodyNode relative to + /// the parent BodyNode expressed in the child BodyNode frame + virtual const math::Jacobian getRelativeJacobianTimeDeriv() const = 0; /// Get constraint wrench expressed in body node frame virtual Eigen::Vector6d getBodyConstraintWrench() const = 0; @@ -662,33 +685,60 @@ class Joint : public virtual common::Subject, /// \{ \name Recursive dynamics routines //---------------------------------------------------------------------------- - /// Update transformation from parent BodyNode to child BodyNode - virtual void updateLocalTransform() const = 0; + /// Deprecated. Use updateRelativeTransform() instead. + DEPRECATED(6.0) + void updateLocalTransform() const; + + /// Deprecated. Use updateRelativeSpatialVelocity() instead. + DEPRECATED(6.0) + void updateLocalSpatialVelocity() const; + + /// Deprecated. Use updateRelativeSpatialAcceleration() instead. + DEPRECATED(6.0) + void updateLocalSpatialAcceleration() const; + + /// Deprecated. Use updateRelativePrimaryAcceleration() instead. + DEPRECATED(6.0) + void updateLocalPrimaryAcceleration() const; + + /// Deprecated. Use updateRelativeJacobian() instead. + DEPRECATED(6.0) + void updateLocalJacobian(bool mandatory = true) const; + + /// Deprecated. Use updateRelativeJacobianTimeDeriv() instead. + DEPRECATED(6.0) + void updateLocalJacobianTimeDeriv() const; + + /// Update transform of the child BodyNode relative to the parent BodyNode + /// expressed in the child BodyNode frame + virtual void updateRelativeTransform() const = 0; - /// Update velocity from the parent BodyNode to the child BodyNode - virtual void updateLocalSpatialVelocity() const = 0; + /// Update spatial velocity of the child BodyNode relative to the parent + /// BodyNode expressed in the child BodyNode frame + virtual void updateRelativeSpatialVelocity() const = 0; - /// Update acceleration from the parent BodyNode to the child BodyNode - virtual void updateLocalSpatialAcceleration() const = 0; + /// Update spatial acceleration of the child BodyNode relative to the parent + /// BodyNode expressed in the child BodyNode frame + virtual void updateRelativeSpatialAcceleration() const = 0; - /// Update the J * q_dd of this joint - virtual void updateLocalPrimaryAcceleration() const = 0; + /// Update J * \ddot{q} of this joint + virtual void updateRelativePrimaryAcceleration() const = 0; - /// Update generalized Jacobian from parent body node to child body - /// node w.r.t. local generalized coordinate + /// Update spatial Jacobian of the child BodyNode relative to the parent + /// BodyNode expressed in the child BodyNode frame /// - /// The _mandatory argument can be set to false if the Jacobian update request - /// is due to a change in Joint positions, because not all Joint types have a - /// Local Jacobian that depends on their Joint positions, so a Local Jacobian - /// update would not actually be required. - virtual void updateLocalJacobian(bool _mandatory=true) const = 0; - - /// Update time derivative of generalized Jacobian from parent body - /// node to child body node w.r.t. local generalized coordinate + /// \param[in] mandatory This argument can be set to false if the Jacobian + /// update request is due to a change in Joint positions, because not all + /// Joint types have a relative Jacobian that depends on their Joint + /// positions, so a relative Jacobian update would not actually be required. + virtual void updateRelativeJacobian(bool mandatory = true) const = 0; + + /// Update time derivative of spatial Jacobian of the child BodyNode relative + /// to the parent BodyNode expressed in the child BodyNode frame /// - /// If the Local Jacobian Time Derivative of this Joint is zero, then this + /// If the relative Jacobian time derivative of this Joint is zero, then this /// function will be a no op. - virtual void updateLocalJacobianTimeDeriv() const = 0; + virtual void updateRelativeJacobianTimeDeriv() const = 0; /// Tells the Skeleton to update the articulated inertia if it needs updating void updateArticulatedInertia() const; @@ -839,51 +889,52 @@ class Joint : public virtual common::Subject, /// Child BodyNode pointer that this Joint belongs to BodyNode* mChildBodyNode; - /// Local transformation + /// Relative transformation of the child BodyNode relative to the parent + /// BodyNode expressed in the child BodyNode frame /// - /// Do not use directly! Use getLocalTransform() to access this + /// Do not use directly! Use getRelativeTransform() to access this mutable Eigen::Isometry3d mT; /// Relative spatial velocity from parent BodyNode to child BodyNode where the /// velocity is expressed in child body Frame /// - /// Do not use directly! Use getLocalSpatialVelocity() to access this + /// Do not use directly! Use getRelativeSpatialVelocity() to access this mutable Eigen::Vector6d mSpatialVelocity; /// Relative spatial acceleration from parent BodyNode to child BodyNode where /// the acceleration is expressed in the child body Frame /// - /// Do not use directly! Use getLocalSpatialAcceleration() to access this + /// Do not use directly! Use getRelativeSpatialAcceleration() to access this mutable Eigen::Vector6d mSpatialAcceleration; /// J * q_dd /// - /// Do not use directly! Use getLocalPrimaryAcceleration() to access this + /// Do not use directly! Use getRelativePrimaryAcceleration() to access this mutable Eigen::Vector6d mPrimaryAcceleration; /// True iff this joint's position has changed since the last call to - /// getLocalTransform() + /// getRelativeTransform() mutable bool mNeedTransformUpdate; /// True iff this joint's position or velocity has changed since the last call - /// to getLocalSpatialVelocity() + /// to getRelativeSpatialVelocity() mutable bool mNeedSpatialVelocityUpdate; /// True iff this joint's position, velocity, or acceleration has changed - /// since the last call to getLocalSpatialAcceleration() + /// since the last call to getRelativeSpatialAcceleration() mutable bool mNeedSpatialAccelerationUpdate; /// True iff this joint's position, velocity, or acceleration has changed - /// since the last call to getLocalPrimaryAcceleration() + /// since the last call to getRelativePrimaryAcceleration() mutable bool mNeedPrimaryAccelerationUpdate; - /// True iff this joint's local Jacobian has not been updated since the last + /// True iff this joint's relative Jacobian has not been updated since the last /// position change - mutable bool mIsLocalJacobianDirty; + mutable bool mIsRelativeJacobianDirty; - /// True iff this joint's local Jacobian time derivative has not been updated + /// True iff this joint's relative Jacobian time derivative has not been updated /// since the last position or velocity change - mutable bool mIsLocalJacobianTimeDerivDirty; + mutable bool mIsRelativeJacobianTimeDerivDirty; public: diff --git a/dart/dynamics/MetaSkeleton.cpp b/dart/dynamics/MetaSkeleton.cpp index effa7ca8a15c1..6fcb351320408 100644 --- a/dart/dynamics/MetaSkeleton.cpp +++ b/dart/dynamics/MetaSkeleton.cpp @@ -420,6 +420,21 @@ void MetaSkeleton::setPositionLowerLimit(std::size_t _index, double _position) this, _index, _position, "setPositionLowerLimit"); } +//============================================================================== +void MetaSkeleton::setPositionLowerLimits(const Eigen::VectorXd& positions) +{ + setAllValuesFromVector<&DegreeOfFreedom::setPositionLowerLimit>( + this, positions, "setPositionLowerLimits", "positions"); +} + +//============================================================================== +void MetaSkeleton::setPositionLowerLimits( + const std::vector& indices, const Eigen::VectorXd& positions) +{ + setValuesFromVector<&DegreeOfFreedom::setPositionLowerLimit>( + this, indices, positions, "setPositionLowerLimits", "positions"); +} + //============================================================================== double MetaSkeleton::getPositionLowerLimit(std::size_t _index) const { @@ -427,6 +442,21 @@ double MetaSkeleton::getPositionLowerLimit(std::size_t _index) const this, _index, "getPositionLowerLimit"); } +//============================================================================== +Eigen::VectorXd MetaSkeleton::getPositionLowerLimits() const +{ + return getValuesFromAllDofs<&DegreeOfFreedom::getPositionLowerLimit>( + this, "getPositionLowerLimits"); +} + +//============================================================================== +Eigen::VectorXd MetaSkeleton::getPositionLowerLimits( + const std::vector& indices) const +{ + return getValuesFromVector<&DegreeOfFreedom::getPositionLowerLimit>( + this, indices, "getPositionLowerLimits"); +} + //============================================================================== void MetaSkeleton::setPositionUpperLimit(std::size_t _index, double _position) { @@ -434,6 +464,21 @@ void MetaSkeleton::setPositionUpperLimit(std::size_t _index, double _position) this, _index, _position, "setPositionUpperLimit"); } +//============================================================================== +void MetaSkeleton::setPositionUpperLimits(const Eigen::VectorXd& positions) +{ + setAllValuesFromVector<&DegreeOfFreedom::setPositionUpperLimit>( + this, positions, "setPositionUpperLimits", "positions"); +} + +//============================================================================== +void MetaSkeleton::setPositionUpperLimits( + const std::vector& indices, const Eigen::VectorXd& positions) +{ + setValuesFromVector<&DegreeOfFreedom::setPositionUpperLimit>( + this, indices, positions, "setPositionUpperLimits", "positions"); +} + //============================================================================== double MetaSkeleton::getPositionUpperLimit(std::size_t _index) const { @@ -441,6 +486,21 @@ double MetaSkeleton::getPositionUpperLimit(std::size_t _index) const this, _index, "getPositionUpperLimit"); } +//============================================================================== +Eigen::VectorXd MetaSkeleton::getPositionUpperLimits() const +{ + return getValuesFromAllDofs<&DegreeOfFreedom::getPositionUpperLimit>( + this, "getPositionUpperLimits"); +} + +//============================================================================== +Eigen::VectorXd MetaSkeleton::getPositionUpperLimits( + const std::vector& indices) const +{ + return getValuesFromVector<&DegreeOfFreedom::getPositionUpperLimit>( + this, indices, "getPositionUpperLimits"); +} + //============================================================================== void MetaSkeleton::setVelocity(std::size_t _index, double _velocity) { @@ -497,6 +557,21 @@ void MetaSkeleton::setVelocityLowerLimit(std::size_t _index, double _velocity) this, _index, _velocity, "setVelocityLowerLimit"); } +//============================================================================== +void MetaSkeleton::setVelocityLowerLimits(const Eigen::VectorXd& velocities) +{ + setAllValuesFromVector<&DegreeOfFreedom::setVelocityLowerLimit>( + this, velocities, "setVelocityLowerLimits", "velocities"); +} + +//============================================================================== +void MetaSkeleton::setVelocityLowerLimits( + const std::vector& indices, const Eigen::VectorXd& velocities) +{ + setValuesFromVector<&DegreeOfFreedom::setVelocityLowerLimit>( + this, indices, velocities, "setVelocityLowerLimits", "velocities"); +} + //============================================================================== double MetaSkeleton::getVelocityLowerLimit(std::size_t _index) { @@ -504,6 +579,21 @@ double MetaSkeleton::getVelocityLowerLimit(std::size_t _index) this, _index, "getVelocityLowerLimit"); } +//============================================================================== +Eigen::VectorXd MetaSkeleton::getVelocityLowerLimits() const +{ + return getValuesFromAllDofs<&DegreeOfFreedom::getVelocityLowerLimit>( + this, "getVelocityLowerLimits"); +} + +//============================================================================== +Eigen::VectorXd MetaSkeleton::getVelocityLowerLimits( + const std::vector& indices) const +{ + return getValuesFromVector<&DegreeOfFreedom::getVelocityLowerLimit>( + this, indices, "getVelocityLowerLimits"); +} + //============================================================================== void MetaSkeleton::setVelocityUpperLimit(std::size_t _index, double _velocity) { @@ -511,6 +601,21 @@ void MetaSkeleton::setVelocityUpperLimit(std::size_t _index, double _velocity) this, _index, _velocity, "setVelocityUpperLimit"); } +//============================================================================== +void MetaSkeleton::setVelocityUpperLimits(const Eigen::VectorXd& velocities) +{ + setAllValuesFromVector<&DegreeOfFreedom::setVelocityUpperLimit>( + this, velocities, "setVelocityUpperLimits", "velocities"); +} + +//============================================================================== +void MetaSkeleton::setVelocityUpperLimits( + const std::vector& indices, const Eigen::VectorXd& velocities) +{ + setValuesFromVector<&DegreeOfFreedom::setVelocityUpperLimit>( + this, indices, velocities, "setVelocityUpperLimits", "velocities"); +} + //============================================================================== double MetaSkeleton::getVelocityUpperLimit(std::size_t _index) { @@ -518,6 +623,21 @@ double MetaSkeleton::getVelocityUpperLimit(std::size_t _index) this, _index, "getVelocityUpperLimit"); } +//============================================================================== +Eigen::VectorXd MetaSkeleton::getVelocityUpperLimits() const +{ + return getValuesFromAllDofs<&DegreeOfFreedom::getVelocityUpperLimit>( + this, "getVelocityUpperLimits"); +} + +//============================================================================== +Eigen::VectorXd MetaSkeleton::getVelocityUpperLimits( + const std::vector& indices) const +{ + return getValuesFromVector<&DegreeOfFreedom::getVelocityUpperLimit>( + this, indices, "getVelocityUpperLimits"); +} + //============================================================================== void MetaSkeleton::setAcceleration(std::size_t _index, double _acceleration) { @@ -575,6 +695,24 @@ void MetaSkeleton::setAccelerationLowerLimit(std::size_t _index, double _acceler this, _index, _acceleration, "setAccelerationLowerLimit"); } +//============================================================================== +void MetaSkeleton::setAccelerationLowerLimits( + const Eigen::VectorXd& accelerations) +{ + setAllValuesFromVector<&DegreeOfFreedom::setAccelerationLowerLimit>( + this, accelerations, "setAccelerationLowerLimits", "accelerations"); +} + +//============================================================================== +void MetaSkeleton::setAccelerationLowerLimits( + const std::vector& indices, + const Eigen::VectorXd& accelerations) +{ + setValuesFromVector<&DegreeOfFreedom::setAccelerationLowerLimit>( + this, indices, accelerations, "setAccelerationLowerLimits", + "accelerations"); +} + //============================================================================== double MetaSkeleton::getAccelerationLowerLimit(std::size_t _index) const { @@ -582,6 +720,21 @@ double MetaSkeleton::getAccelerationLowerLimit(std::size_t _index) const this, _index, "getAccelerationLowerLimit"); } +//============================================================================== +Eigen::VectorXd MetaSkeleton::getAccelerationLowerLimits() const +{ + return getValuesFromAllDofs<&DegreeOfFreedom::getAccelerationLowerLimit>( + this, "getAccelerationLowerLimits"); +} + +//============================================================================== +Eigen::VectorXd MetaSkeleton::getAccelerationLowerLimits( + const std::vector& indices) const +{ + return getValuesFromVector<&DegreeOfFreedom::getAccelerationLowerLimit>( + this, indices, "getAccelerationLowerLimits"); +} + //============================================================================== void MetaSkeleton::setAccelerationUpperLimit(std::size_t _index, double _acceleration) { @@ -589,6 +742,24 @@ void MetaSkeleton::setAccelerationUpperLimit(std::size_t _index, double _acceler this, _index, _acceleration, "setAccelerationUpperLimit"); } +//============================================================================== +void MetaSkeleton::setAccelerationUpperLimits( + const Eigen::VectorXd& accelerations) +{ + setAllValuesFromVector<&DegreeOfFreedom::setAccelerationUpperLimit>( + this, accelerations, "setAccelerationUpperLimits", "accelerations"); +} + +//============================================================================== +void MetaSkeleton::setAccelerationUpperLimits( + const std::vector& indices, + const Eigen::VectorXd& accelerations) +{ + setValuesFromVector<&DegreeOfFreedom::setAccelerationUpperLimit>( + this, indices, accelerations, "setAccelerationUpperLimits", + "accelerations"); +} + //============================================================================== double MetaSkeleton::getAccelerationUpperLimit(std::size_t _index) const { @@ -596,6 +767,21 @@ double MetaSkeleton::getAccelerationUpperLimit(std::size_t _index) const this, _index, "getAccelerationUpperLimit"); } +//============================================================================== +Eigen::VectorXd MetaSkeleton::getAccelerationUpperLimits() const +{ + return getValuesFromAllDofs<&DegreeOfFreedom::getAccelerationUpperLimit>( + this, "getAccelerationUpperLimits"); +} + +//============================================================================== +Eigen::VectorXd MetaSkeleton::getAccelerationUpperLimits( + const std::vector& indices) const +{ + return getValuesFromVector<&DegreeOfFreedom::getAccelerationUpperLimit>( + this, indices, "getAccelerationUpperLimits"); +} + //============================================================================== void MetaSkeleton::setForce(std::size_t _index, double _force) { @@ -654,6 +840,21 @@ void MetaSkeleton::setForceLowerLimit(std::size_t _index, double _force) this, _index, _force, "setForceLowerLimit"); } +//============================================================================== +void MetaSkeleton::setForceLowerLimits(const Eigen::VectorXd& forces) +{ + setAllValuesFromVector<&DegreeOfFreedom::setForceLowerLimit>( + this, forces, "setForceLowerLimits", "forces"); +} + +//============================================================================== +void MetaSkeleton::setForceLowerLimits(const std::vector& indices, + const Eigen::VectorXd& forces) +{ + setValuesFromVector<&DegreeOfFreedom::setForceLowerLimit>( + this, indices, forces, "setForceLowerLimits", "forces"); +} + //============================================================================== double MetaSkeleton::getForceLowerLimit(std::size_t _index) const { @@ -661,6 +862,21 @@ double MetaSkeleton::getForceLowerLimit(std::size_t _index) const this, _index, "getForceLowerLimit"); } +//============================================================================== +Eigen::VectorXd MetaSkeleton::getForceLowerLimits() const +{ + return getValuesFromAllDofs<&DegreeOfFreedom::getForceLowerLimit>( + this, "getForceLowerLimits"); +} + +//============================================================================== +Eigen::VectorXd MetaSkeleton::getForceLowerLimits( + const std::vector& indices) const +{ + return getValuesFromVector<&DegreeOfFreedom::getForceLowerLimit>( + this, indices, "getForceLowerLimits"); +} + //============================================================================== void MetaSkeleton::setForceUpperLimit(std::size_t _index, double _force) { @@ -668,6 +884,21 @@ void MetaSkeleton::setForceUpperLimit(std::size_t _index, double _force) this, _index, _force, "setForceUpperLimit"); } +//============================================================================== +void MetaSkeleton::setForceUpperLimits(const Eigen::VectorXd& forces) +{ + setAllValuesFromVector<&DegreeOfFreedom::setForceUpperLimit>( + this, forces, "setForceUpperLimits", "forces"); +} + +//============================================================================== +void MetaSkeleton::setForceUpperLimits(const std::vector& indices, + const Eigen::VectorXd& forces) +{ + setValuesFromVector<&DegreeOfFreedom::setForceUpperLimit>( + this, indices, forces, "setForceUpperLimits", "forces"); +} + //============================================================================== double MetaSkeleton::getForceUpperLimit(std::size_t _index) const { @@ -675,6 +906,21 @@ double MetaSkeleton::getForceUpperLimit(std::size_t _index) const this, _index, "getForceUpperLimit"); } +//============================================================================== +Eigen::VectorXd MetaSkeleton::getForceUpperLimits() const +{ + return getValuesFromAllDofs<&DegreeOfFreedom::getForceUpperLimit>( + this, "getForceUpperLimits"); +} + +//============================================================================== +Eigen::VectorXd MetaSkeleton::getForceUpperLimits( + const std::vector& indices) const +{ + return getValuesFromVector<&DegreeOfFreedom::getForceUpperLimit>( + this, indices, "getForceUpperLimits"); +} + //============================================================================== Eigen::VectorXd MetaSkeleton::getVelocityChanges() const { diff --git a/dart/dynamics/MetaSkeleton.hpp b/dart/dynamics/MetaSkeleton.hpp index c78b9faa19be3..c9ada27fd198a 100644 --- a/dart/dynamics/MetaSkeleton.hpp +++ b/dart/dynamics/MetaSkeleton.hpp @@ -208,15 +208,43 @@ class MetaSkeleton : public common::Subject /// Set the lower limit of a generalized coordinate's position void setPositionLowerLimit(std::size_t _index, double _position); + /// Set the lower limits for all generalized coordinates + void setPositionLowerLimits(const Eigen::VectorXd& positions); + + /// Set the lower limits for a subset of the generalized coordinates + void setPositionLowerLimits(const std::vector& indices, + const Eigen::VectorXd& positions); + /// Get the lower limit of a generalized coordinate's position double getPositionLowerLimit(std::size_t _index) const; + /// Get the lower limits for all generalized coordinates + Eigen::VectorXd getPositionLowerLimits() const; + + /// Get the lower limits for a subset of the generalized coordinates + Eigen::VectorXd getPositionLowerLimits( + const std::vector& indices) const; + /// Set the upper limit of a generalized coordainte's position void setPositionUpperLimit(std::size_t _index, double _position); + /// Set the upper limits for all generalized coordinates + void setPositionUpperLimits(const Eigen::VectorXd& positions); + + /// Set the upper limits for a subset of the generalized coordinates + void setPositionUpperLimits(const std::vector& indices, + const Eigen::VectorXd& positions); + /// Get the upper limit of a generalized coordinate's position double getPositionUpperLimit(std::size_t _index) const; + /// Get the upper limits for all generalized coordinates + Eigen::VectorXd getPositionUpperLimits() const; + + /// Get the upper limits for a subset of the generalized coordinates + Eigen::VectorXd getPositionUpperLimits( + const std::vector& indices) const; + /// \} //---------------------------------------------------------------------------- @@ -248,15 +276,47 @@ class MetaSkeleton : public common::Subject /// Set the lower limit of a generalized coordinate's velocity void setVelocityLowerLimit(std::size_t _index, double _velocity); + /// Set the lower limits for all generalized coordinates's velocity + void setVelocityLowerLimits(const Eigen::VectorXd& velocities); + + /// Set the lower limits for a subset of the generalized coordinates's + /// velocity + void setVelocityLowerLimits(const std::vector& indices, + const Eigen::VectorXd& velocities); + /// Get the lower limit of a generalized coordinate's velocity double getVelocityLowerLimit(std::size_t _index); + /// Get the lower limits for all generalized coordinates's velocity + Eigen::VectorXd getVelocityLowerLimits() const; + + /// Get the lower limits for a subset of the generalized coordinates's + /// velocity + Eigen::VectorXd getVelocityLowerLimits( + const std::vector& indices) const; + /// Set the upper limit of a generalized coordinate's velocity void setVelocityUpperLimit(std::size_t _index, double _velocity); + /// Set the upper limits for all generalized coordinates's velocity + void setVelocityUpperLimits(const Eigen::VectorXd& velocities); + + /// Set the upper limits for a subset of the generalized coordinates's + /// velocity + void setVelocityUpperLimits(const std::vector& indices, + const Eigen::VectorXd& velocities); + /// Get the upper limit of a generalized coordinate's velocity double getVelocityUpperLimit(std::size_t _index); + /// Get the upper limits for all generalized coordinates's velocity + Eigen::VectorXd getVelocityUpperLimits() const; + + /// Get the upper limits for a subset of the generalized coordinates's + /// velocity + Eigen::VectorXd getVelocityUpperLimits( + const std::vector& indices) const; + /// \} //---------------------------------------------------------------------------- @@ -288,15 +348,47 @@ class MetaSkeleton : public common::Subject /// Set the lower limit of a generalized coordinate's acceleration void setAccelerationLowerLimit(std::size_t _index, double _acceleration); + /// Set the lower limits for all generalized coordinates's acceleration + void setAccelerationLowerLimits(const Eigen::VectorXd& accelerations); + + /// Set the lower limits for a subset of the generalized coordinates's + /// acceleration + void setAccelerationLowerLimits(const std::vector& indices, + const Eigen::VectorXd& accelerations); + /// Get the lower limit of a generalized coordinate's acceleration double getAccelerationLowerLimit(std::size_t _index) const; + /// Get the lower limits for all generalized coordinates's acceleration + Eigen::VectorXd getAccelerationLowerLimits() const; + + /// Get the lower limits for a subset of the generalized coordinates's + /// acceleration + Eigen::VectorXd getAccelerationLowerLimits( + const std::vector& indices) const; + /// Set the upper limit of a generalized coordinate's acceleration void setAccelerationUpperLimit(std::size_t _index, double _acceleration); + /// Set the upper limits for all generalized coordinates's acceleration + void setAccelerationUpperLimits(const Eigen::VectorXd& accelerations); + + /// Set the upper limits for a subset of the generalized coordinates's + /// acceleration + void setAccelerationUpperLimits(const std::vector& indices, + const Eigen::VectorXd& accelerations); + /// Get the upper limit of a generalized coordinate's acceleration double getAccelerationUpperLimit(std::size_t _index) const; + /// Get the upper limits for all generalized coordinates's acceleration + Eigen::VectorXd getAccelerationUpperLimits() const; + + /// Get the upper limits for a subset of the generalized coordinates's + /// acceleration + Eigen::VectorXd getAccelerationUpperLimits( + const std::vector& indices) const; + /// \} //---------------------------------------------------------------------------- @@ -328,15 +420,43 @@ class MetaSkeleton : public common::Subject /// Set the lower limit of a generalized coordinate's force void setForceLowerLimit(std::size_t _index, double _force); + /// Set the lower limits for all generalized coordinates's force + void setForceLowerLimits(const Eigen::VectorXd& forces); + + /// Set the lower limits for a subset of the generalized coordinates's force + void setForceLowerLimits(const std::vector& indices, + const Eigen::VectorXd& forces); + /// Get the lower limit of a generalized coordinate's force double getForceLowerLimit(std::size_t _index) const; + /// Get the lower limits for all generalized coordinates's force + Eigen::VectorXd getForceLowerLimits() const; + + /// Get the lower limits for a subset of the generalized coordinates's force + Eigen::VectorXd getForceLowerLimits( + const std::vector& indices) const; + /// Set the upper limit of a generalized coordinate's force void setForceUpperLimit(std::size_t _index, double _force); + /// Set the upperlimits for all generalized coordinates's force + void setForceUpperLimits(const Eigen::VectorXd& forces); + + /// Set the upper limits for a subset of the generalized coordinates's force + void setForceUpperLimits(const std::vector& indices, + const Eigen::VectorXd& forces); + /// Get the upper limit of a generalized coordinate's force double getForceUpperLimit(std::size_t _index) const; + /// Get the upper limits for all generalized coordinates's force + Eigen::VectorXd getForceUpperLimits() const; + + /// Get the upper limits for a subset of the generalized coordinates's force + Eigen::VectorXd getForceUpperLimits( + const std::vector& indices) const; + /// \} //---------------------------------------------------------------------------- diff --git a/dart/dynamics/MultiDofJoint.hpp b/dart/dynamics/MultiDofJoint.hpp index 78d3162496bcd..1dd201a240039 100644 --- a/dart/dynamics/MultiDofJoint.hpp +++ b/dart/dynamics/MultiDofJoint.hpp @@ -433,24 +433,24 @@ class MultiDofJoint : public detail::MultiDofJointBase< MultiDofJoint, DOF //---------------------------------------------------------------------------- // Documentation inherited - const math::Jacobian getLocalJacobian() const override; + const math::Jacobian getRelativeJacobian() const override; - /// Fixed-size version of getLocalJacobian() - const Eigen::Matrix& getLocalJacobianStatic() const; + /// Fixed-size version of getRelativeJacobian() + const Eigen::Matrix& getRelativeJacobianStatic() const; // Documentation inherited - math::Jacobian getLocalJacobian( + math::Jacobian getRelativeJacobian( const Eigen::VectorXd& _positions) const override; - /// Fixed-size version of getLocalJacobian() - virtual Eigen::Matrix getLocalJacobianStatic( + /// Fixed-size version of getRelativeJacobian() + virtual Eigen::Matrix getRelativeJacobianStatic( const Eigen::Matrix& _positions) const = 0; // Documentation inherited - const math::Jacobian getLocalJacobianTimeDeriv() const override; + const math::Jacobian getRelativeJacobianTimeDeriv() const override; - /// Fixed-size version of getLocalJacobianTimeDeriv() - const Eigen::Matrix& getLocalJacobianTimeDerivStatic() const; + /// Fixed-size version of getRelativeJacobianTimeDeriv() + const Eigen::Matrix& getRelativeJacobianTimeDerivStatic() const; /// Get the inverse of the projected articulated inertia const Eigen::Matrix& getInvProjArtInertia() const; @@ -460,13 +460,13 @@ class MultiDofJoint : public detail::MultiDofJointBase< MultiDofJoint, DOF const Eigen::Matrix& getInvProjArtInertiaImplicit() const; // Documentation inherited - void updateLocalSpatialVelocity() const override; + void updateRelativeSpatialVelocity() const override; // Documentation inherited - void updateLocalSpatialAcceleration() const override; + void updateRelativeSpatialAcceleration() const override; // Documentation inherited - void updateLocalPrimaryAcceleration() const override; + void updateRelativePrimaryAcceleration() const override; // Documentation inherited void addVelocityTo(Eigen::Vector6d& _vel) override; @@ -622,12 +622,12 @@ class MultiDofJoint : public detail::MultiDofJointBase< MultiDofJoint, DOF /// Spatial Jacobian expressed in the child body frame /// - /// Do not use directly! Use getLocalJacobianStatic() to access this quantity + /// Do not use directly! Use getRelativeJacobianStatic() to access this quantity mutable Eigen::Matrix mJacobian; /// Time derivative of spatial Jacobian expressed in the child body frame /// - /// Do not use directly! Use getLocalJacobianTimeDerivStatic() to access this + /// Do not use directly! Use getRelativeJacobianTimeDerivStatic() to access this /// quantity mutable Eigen::Matrix mJacobianDeriv; diff --git a/dart/dynamics/PlanarJoint.cpp b/dart/dynamics/PlanarJoint.cpp index 7cc172f9aca3c..5f986f9fc458f 100644 --- a/dart/dynamics/PlanarJoint.cpp +++ b/dart/dynamics/PlanarJoint.cpp @@ -71,7 +71,7 @@ void PlanarJoint::setAspectProperties(const AspectProperties& properties) { mAspectProperties = properties; Joint::notifyPositionUpdate(); - updateLocalJacobian(true); + updateRelativeJacobian(true); Joint::incrementVersion(); } @@ -192,7 +192,7 @@ const Eigen::Vector3d& PlanarJoint::getTranslationalAxis2() const } //============================================================================== -Eigen::Matrix PlanarJoint::getLocalJacobianStatic( +Eigen::Matrix PlanarJoint::getRelativeJacobianStatic( const Eigen::Vector3d& _positions) const { Eigen::Matrix J = Eigen::Matrix::Zero(); @@ -269,7 +269,7 @@ void PlanarJoint::updateDegreeOfFreedomNames() } //============================================================================== -void PlanarJoint::updateLocalTransform() const +void PlanarJoint::updateRelativeTransform() const { const Eigen::Vector3d& positions = getPositionsStatic(); mT = Joint::mAspectProperties.mT_ParentBodyToJoint @@ -283,20 +283,20 @@ void PlanarJoint::updateLocalTransform() const } //============================================================================== -void PlanarJoint::updateLocalJacobian(bool) const +void PlanarJoint::updateRelativeJacobian(bool) const { - mJacobian = getLocalJacobianStatic(getPositionsStatic()); + mJacobian = getRelativeJacobianStatic(getPositionsStatic()); } //============================================================================== -void PlanarJoint::updateLocalJacobianTimeDeriv() const +void PlanarJoint::updateRelativeJacobianTimeDeriv() const { Eigen::Matrix J = Eigen::Matrix::Zero(); J.block<3, 1>(3, 0) = mAspectProperties.mTransAxis1; J.block<3, 1>(3, 1) = mAspectProperties.mTransAxis2; J.block<3, 1>(0, 2) = mAspectProperties.mRotAxis; - const Eigen::Matrix& Jacobian = getLocalJacobianStatic(); + const Eigen::Matrix& Jacobian = getRelativeJacobianStatic(); const Eigen::Vector3d& velocities = getVelocitiesStatic(); mJacobianDeriv.col(0) = -math::ad(Jacobian.col(2) * velocities[2], diff --git a/dart/dynamics/PlanarJoint.hpp b/dart/dynamics/PlanarJoint.hpp index 8603fdbe3d4fd..85a170d92d11c 100644 --- a/dart/dynamics/PlanarJoint.hpp +++ b/dart/dynamics/PlanarJoint.hpp @@ -131,7 +131,7 @@ class PlanarJoint : public detail::PlanarJointBase const Eigen::Vector3d& getTranslationalAxis2() const; // Documentation inherited - Eigen::Matrix getLocalJacobianStatic( + Eigen::Matrix getRelativeJacobianStatic( const Eigen::Vector3d& _positions) const override; protected: @@ -142,20 +142,20 @@ class PlanarJoint : public detail::PlanarJointBase // Documentation inherited Joint* clone() const override; - using MultiDofJoint::getLocalJacobianStatic; + using MultiDofJoint::getRelativeJacobianStatic; /// Set the names of this joint's DegreesOfFreedom. Used during construction /// and when the Plane type is changed. void updateDegreeOfFreedomNames() override; // Documentation inherited - void updateLocalTransform() const override; + void updateRelativeTransform() const override; // Documentation inherited - void updateLocalJacobian(bool =true) const override; + void updateRelativeJacobian(bool =true) const override; // Documentation inherited - void updateLocalJacobianTimeDeriv() const override; + void updateRelativeJacobianTimeDeriv() const override; public: // To get byte-aligned Eigen vectors diff --git a/dart/dynamics/PrismaticJoint.cpp b/dart/dynamics/PrismaticJoint.cpp index 36374e67ca522..7f3dbaf83d79c 100644 --- a/dart/dynamics/PrismaticJoint.cpp +++ b/dart/dynamics/PrismaticJoint.cpp @@ -129,7 +129,7 @@ void PrismaticJoint::setAxis(const Eigen::Vector3d& _axis) mAspectProperties.mAxis = _axis.normalized(); Joint::notifyPositionUpdate(); - updateLocalJacobian(); + updateRelativeJacobian(); Joint::incrementVersion(); } @@ -157,7 +157,7 @@ Joint* PrismaticJoint::clone() const } //============================================================================== -void PrismaticJoint::updateLocalTransform() const +void PrismaticJoint::updateRelativeTransform() const { mT = Joint::mAspectProperties.mT_ParentBodyToJoint * Eigen::Translation3d(getAxis() * getPositionStatic()) @@ -168,7 +168,7 @@ void PrismaticJoint::updateLocalTransform() const } //============================================================================== -void PrismaticJoint::updateLocalJacobian(bool _mandatory) const +void PrismaticJoint::updateRelativeJacobian(bool _mandatory) const { if(_mandatory) { @@ -180,7 +180,7 @@ void PrismaticJoint::updateLocalJacobian(bool _mandatory) const } //============================================================================== -void PrismaticJoint::updateLocalJacobianTimeDeriv() const +void PrismaticJoint::updateRelativeJacobianTimeDeriv() const { // Time derivative of prismatic joint is always zero assert(mJacobianDeriv == Eigen::Vector6d::Zero()); diff --git a/dart/dynamics/PrismaticJoint.hpp b/dart/dynamics/PrismaticJoint.hpp index 910239c493766..e3fa1a7cb6e01 100644 --- a/dart/dynamics/PrismaticJoint.hpp +++ b/dart/dynamics/PrismaticJoint.hpp @@ -103,13 +103,13 @@ class PrismaticJoint : public detail::PrismaticJointBase Joint* clone() const override; // Documentation inherited - void updateLocalTransform() const override; + void updateRelativeTransform() const override; // Documentation inherited - void updateLocalJacobian(bool _mandatory=true) const override; + void updateRelativeJacobian(bool _mandatory=true) const override; // Documentation inherited - void updateLocalJacobianTimeDeriv() const override; + void updateRelativeJacobianTimeDeriv() const override; public: // To get byte-aligned Eigen vectors diff --git a/dart/dynamics/RevoluteJoint.cpp b/dart/dynamics/RevoluteJoint.cpp index 140c6c223b761..8b931a913de21 100644 --- a/dart/dynamics/RevoluteJoint.cpp +++ b/dart/dynamics/RevoluteJoint.cpp @@ -130,7 +130,7 @@ void RevoluteJoint::setAxis(const Eigen::Vector3d& _axis) mAspectProperties.mAxis = _axis.normalized(); Joint::notifyPositionUpdate(); - updateLocalJacobian(); + updateRelativeJacobian(); Joint::incrementVersion(); } @@ -158,7 +158,7 @@ Joint* RevoluteJoint::clone() const } //============================================================================== -void RevoluteJoint::updateLocalTransform() const +void RevoluteJoint::updateRelativeTransform() const { mT = Joint::mAspectProperties.mT_ParentBodyToJoint * math::expAngular(getAxis() * getPositionStatic()) @@ -169,7 +169,7 @@ void RevoluteJoint::updateLocalTransform() const } //============================================================================== -void RevoluteJoint::updateLocalJacobian(bool _mandatory) const +void RevoluteJoint::updateRelativeJacobian(bool _mandatory) const { if(_mandatory) { @@ -182,7 +182,7 @@ void RevoluteJoint::updateLocalJacobian(bool _mandatory) const } //============================================================================== -void RevoluteJoint::updateLocalJacobianTimeDeriv() const +void RevoluteJoint::updateRelativeJacobianTimeDeriv() const { // Time derivative of revolute joint is always zero assert(mJacobianDeriv == Eigen::Vector6d::Zero()); diff --git a/dart/dynamics/RevoluteJoint.hpp b/dart/dynamics/RevoluteJoint.hpp index bdb7f04340048..d324d8ea273e5 100644 --- a/dart/dynamics/RevoluteJoint.hpp +++ b/dart/dynamics/RevoluteJoint.hpp @@ -104,13 +104,13 @@ class RevoluteJoint : public detail::RevoluteJointBase Joint* clone() const override; // Documentation inherited - void updateLocalTransform() const override; + void updateRelativeTransform() const override; // Documentation inherited - void updateLocalJacobian(bool _mandatory=true) const override; + void updateRelativeJacobian(bool _mandatory=true) const override; // Documentation inherited - void updateLocalJacobianTimeDeriv() const override; + void updateRelativeJacobianTimeDeriv() const override; public: diff --git a/dart/dynamics/ScrewJoint.cpp b/dart/dynamics/ScrewJoint.cpp index f74b38b9fa8cc..3f01149ea3a6f 100644 --- a/dart/dynamics/ScrewJoint.cpp +++ b/dart/dynamics/ScrewJoint.cpp @@ -130,7 +130,7 @@ void ScrewJoint::setAxis(const Eigen::Vector3d& _axis) mAspectProperties.mAxis = _axis.normalized(); Joint::notifyPositionUpdate(); - updateLocalJacobian(); + updateRelativeJacobian(); Joint::incrementVersion(); } @@ -148,7 +148,7 @@ void ScrewJoint::setPitch(double _pitch) mAspectProperties.mPitch = _pitch; Joint::notifyPositionUpdate(); - updateLocalJacobian(); + updateRelativeJacobian(); Joint::incrementVersion(); } @@ -176,7 +176,7 @@ Joint* ScrewJoint::clone() const } //============================================================================== -void ScrewJoint::updateLocalTransform() const +void ScrewJoint::updateRelativeTransform() const { using namespace dart::math::suffixes; @@ -190,7 +190,7 @@ void ScrewJoint::updateLocalTransform() const } //============================================================================== -void ScrewJoint::updateLocalJacobian(bool _mandatory) const +void ScrewJoint::updateRelativeJacobian(bool _mandatory) const { using namespace dart::math::suffixes; @@ -205,7 +205,7 @@ void ScrewJoint::updateLocalJacobian(bool _mandatory) const } //============================================================================== -void ScrewJoint::updateLocalJacobianTimeDeriv() const +void ScrewJoint::updateRelativeJacobianTimeDeriv() const { // Time derivative of screw joint is always zero assert(mJacobianDeriv == Eigen::Vector6d::Zero()); diff --git a/dart/dynamics/ScrewJoint.hpp b/dart/dynamics/ScrewJoint.hpp index 4b3c70e4ba555..1622e446267ae 100644 --- a/dart/dynamics/ScrewJoint.hpp +++ b/dart/dynamics/ScrewJoint.hpp @@ -110,13 +110,13 @@ class ScrewJoint : public detail::ScrewJointBase Joint* clone() const override; // Documentation inherited - void updateLocalTransform() const override; + void updateRelativeTransform() const override; // Documentation inherited - void updateLocalJacobian(bool _mandatory=true) const override; + void updateRelativeJacobian(bool _mandatory=true) const override; // Documentation inherited - void updateLocalJacobianTimeDeriv() const override; + void updateRelativeJacobianTimeDeriv() const override; public: // To get byte-aligned Eigen vectors diff --git a/dart/dynamics/ShapeFrame.cpp b/dart/dynamics/ShapeFrame.cpp index 441d8ae54e5cc..0ec2c677e100a 100644 --- a/dart/dynamics/ShapeFrame.cpp +++ b/dart/dynamics/ShapeFrame.cpp @@ -79,9 +79,8 @@ ShapeFrameProperties::ShapeFrameProperties(const ShapePtr& shape) } // namespace detail //============================================================================== -VisualAspect::VisualAspect(common::Composite* comp, - const PropertiesData& properties) - : VisualAspect::BaseClass(comp, properties) +VisualAspect::VisualAspect(const PropertiesData& properties) + : VisualAspect::BaseClass(properties) { // Do nothing } @@ -165,9 +164,8 @@ bool VisualAspect::isHidden() const //============================================================================== CollisionAspect::CollisionAspect( - common::Composite* comp, const PropertiesData& properties) - : AspectImplementation(comp, properties) + : AspectImplementation(properties) { // Do nothing } @@ -180,9 +178,8 @@ bool CollisionAspect::isCollidable() const //============================================================================== DynamicsAspect::DynamicsAspect( - common::Composite* comp, const PropertiesData& properties) - : BaseClass(comp, properties) + : BaseClass(properties) { // Do nothing } diff --git a/dart/dynamics/ShapeFrame.hpp b/dart/dynamics/ShapeFrame.hpp index 15b0f5da41f15..c99e8c6a5e6af 100644 --- a/dart/dynamics/ShapeFrame.hpp +++ b/dart/dynamics/ShapeFrame.hpp @@ -63,8 +63,7 @@ class VisualAspect final : VisualAspect, detail::VisualAspectProperties, ShapeFrame>; /// Constructor - VisualAspect(common::Composite* comp, - const PropertiesData& properties = PropertiesData()); + VisualAspect(const PropertiesData& properties = PropertiesData()); VisualAspect(const VisualAspect&) = delete; @@ -125,8 +124,7 @@ class CollisionAspect final : public: CollisionAspect(const CollisionAspect &) = delete; - CollisionAspect(dart::common::Composite* comp, - const PropertiesData& properties = PropertiesData()); + CollisionAspect(const PropertiesData& properties = PropertiesData()); DART_COMMON_SET_GET_ASPECT_PROPERTY( bool, Collidable ) // void setCollidable(const bool& value); @@ -151,8 +149,7 @@ class DynamicsAspect final : DynamicsAspect(const DynamicsAspect&) = delete; - DynamicsAspect(dart::common::Composite* comp, - const PropertiesData& properties = PropertiesData()); + DynamicsAspect(const PropertiesData& properties = PropertiesData()); DART_COMMON_SET_GET_ASPECT_PROPERTY( double, FrictionCoeff ) // void setFrictionCoeff(const double& value); diff --git a/dart/dynamics/SingleDofJoint.cpp b/dart/dynamics/SingleDofJoint.cpp index c0e9583d4133f..c4bcb4ca3609c 100644 --- a/dart/dynamics/SingleDofJoint.cpp +++ b/dart/dynamics/SingleDofJoint.cpp @@ -1198,22 +1198,22 @@ void SingleDofJoint::updateDegreeOfFreedomNames() } //============================================================================== -void SingleDofJoint::updateLocalSpatialVelocity() const +void SingleDofJoint::updateRelativeSpatialVelocity() const { - mSpatialVelocity = getLocalJacobianStatic() * getVelocityStatic(); + mSpatialVelocity = getRelativeJacobianStatic() * getVelocityStatic(); } //============================================================================== -void SingleDofJoint::updateLocalSpatialAcceleration() const +void SingleDofJoint::updateRelativeSpatialAcceleration() const { - mSpatialAcceleration = getLocalPrimaryAcceleration() - + getLocalJacobianTimeDerivStatic() * getVelocityStatic(); + mSpatialAcceleration = getRelativePrimaryAcceleration() + + getRelativeJacobianTimeDerivStatic() * getVelocityStatic(); } //============================================================================== -void SingleDofJoint::updateLocalPrimaryAcceleration() const +void SingleDofJoint::updateRelativePrimaryAcceleration() const { - mPrimaryAcceleration = getLocalJacobianStatic() * getAccelerationStatic(); + mPrimaryAcceleration = getRelativeJacobianStatic() * getAccelerationStatic(); } //============================================================================== @@ -1221,52 +1221,52 @@ Eigen::Vector6d SingleDofJoint::getBodyConstraintWrench() const { assert(mChildBodyNode); return mChildBodyNode->getBodyForce() - - getLocalJacobianStatic() * mAspectState.mForce; + - getRelativeJacobianStatic() * mAspectState.mForce; } //============================================================================== -const math::Jacobian SingleDofJoint::getLocalJacobian() const +const math::Jacobian SingleDofJoint::getRelativeJacobian() const { - return getLocalJacobianStatic(); + return getRelativeJacobianStatic(); } //============================================================================== -const Eigen::Vector6d& SingleDofJoint::getLocalJacobianStatic() const +const Eigen::Vector6d& SingleDofJoint::getRelativeJacobianStatic() const { - if(mIsLocalJacobianDirty) + if(mIsRelativeJacobianDirty) { - updateLocalJacobian(false); - mIsLocalJacobianDirty = false; + updateRelativeJacobian(false); + mIsRelativeJacobianDirty = false; } return mJacobian; } //============================================================================== -math::Jacobian SingleDofJoint::getLocalJacobian( +math::Jacobian SingleDofJoint::getRelativeJacobian( const Eigen::VectorXd& /*_positions*/) const { // The Jacobian is always constant w.r.t. the generalized coordinates. - return getLocalJacobianStatic(); + return getRelativeJacobianStatic(); } //============================================================================== -const math::Jacobian SingleDofJoint::getLocalJacobianTimeDeriv() const +const math::Jacobian SingleDofJoint::getRelativeJacobianTimeDeriv() const { - if(mIsLocalJacobianTimeDerivDirty) + if(mIsRelativeJacobianTimeDerivDirty) { - updateLocalJacobianTimeDeriv(); - mIsLocalJacobianTimeDerivDirty = false; + updateRelativeJacobianTimeDeriv(); + mIsRelativeJacobianTimeDerivDirty = false; } return mJacobianDeriv; } //============================================================================== -const Eigen::Vector6d& SingleDofJoint::getLocalJacobianTimeDerivStatic() const +const Eigen::Vector6d& SingleDofJoint::getRelativeJacobianTimeDerivStatic() const { - if(mIsLocalJacobianTimeDerivDirty) + if(mIsRelativeJacobianTimeDerivDirty) { - updateLocalJacobianTimeDeriv(); - mIsLocalJacobianTimeDerivDirty = false; + updateRelativeJacobianTimeDeriv(); + mIsRelativeJacobianTimeDerivDirty = false; } return mJacobianDeriv; } @@ -1289,7 +1289,7 @@ const double& SingleDofJoint::getInvProjArtInertiaImplicit() const void SingleDofJoint::addVelocityTo(Eigen::Vector6d& _vel) { // Add joint velocity to _vel - _vel.noalias() += getLocalJacobianStatic() * getVelocityStatic(); + _vel.noalias() += getRelativeJacobianStatic() * getVelocityStatic(); // Verification assert(!math::isNan(_vel)); @@ -1301,7 +1301,7 @@ void SingleDofJoint::setPartialAccelerationTo( const Eigen::Vector6d& _childVelocity) { // ad(V, S * dq) + dS * dq - _partialAcceleration = math::ad(_childVelocity, getLocalJacobianStatic() + _partialAcceleration = math::ad(_childVelocity, getRelativeJacobianStatic() * getVelocityStatic()) + mJacobianDeriv * getVelocityStatic(); // Verification @@ -1313,14 +1313,14 @@ void SingleDofJoint::setPartialAccelerationTo( void SingleDofJoint::addAccelerationTo(Eigen::Vector6d& _acc) { // - _acc += getLocalJacobianStatic() * getAccelerationStatic(); + _acc += getRelativeJacobianStatic() * getAccelerationStatic(); } //============================================================================== void SingleDofJoint::addVelocityChangeTo(Eigen::Vector6d& _velocityChange) { // - _velocityChange += getLocalJacobianStatic() * mVelocityChange; + _velocityChange += getRelativeJacobianStatic() * mVelocityChange; } //============================================================================== @@ -1352,14 +1352,14 @@ void SingleDofJoint::addChildArtInertiaToDynamic( Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) { // Child body's articulated inertia - Eigen::Vector6d AIS = _childArtInertia * getLocalJacobianStatic(); + Eigen::Vector6d AIS = _childArtInertia * getRelativeJacobianStatic(); Eigen::Matrix6d PI = _childArtInertia; PI.noalias() -= mInvProjArtInertia * AIS * AIS.transpose(); assert(!math::isNan(PI)); // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. - _parentArtInertia += math::transformInertia(getLocalTransform().inverse(), PI); + _parentArtInertia += math::transformInertia(getRelativeTransform().inverse(), PI); } //============================================================================== @@ -1368,7 +1368,7 @@ void SingleDofJoint::addChildArtInertiaToKinematic( { // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. - _parentArtInertia += math::transformInertia(getLocalTransform().inverse(), + _parentArtInertia += math::transformInertia(getRelativeTransform().inverse(), _childArtInertia); } @@ -1401,14 +1401,14 @@ void SingleDofJoint::addChildArtInertiaImplicitToDynamic( Eigen::Matrix6d& _parentArtInertia, const Eigen::Matrix6d& _childArtInertia) { // Child body's articulated inertia - const Eigen::Vector6d AIS = _childArtInertia * getLocalJacobianStatic(); + const Eigen::Vector6d AIS = _childArtInertia * getRelativeJacobianStatic(); Eigen::Matrix6d PI = _childArtInertia; PI.noalias() -= mInvProjArtInertiaImplicit * AIS * AIS.transpose(); assert(!math::isNan(PI)); // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. - _parentArtInertia += math::transformInertia(getLocalTransform().inverse(), PI); + _parentArtInertia += math::transformInertia(getRelativeTransform().inverse(), PI); } //============================================================================== @@ -1417,7 +1417,7 @@ void SingleDofJoint::addChildArtInertiaImplicitToKinematic( { // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. - _parentArtInertia += math::transformInertia(getLocalTransform().inverse(), + _parentArtInertia += math::transformInertia(getRelativeTransform().inverse(), _childArtInertia); } @@ -1448,7 +1448,7 @@ void SingleDofJoint::updateInvProjArtInertiaDynamic( const Eigen::Matrix6d& _artInertia) { // Projected articulated inertia - const Eigen::Vector6d& Jacobian = getLocalJacobianStatic(); + const Eigen::Vector6d& Jacobian = getRelativeJacobianStatic(); double projAI = Jacobian.dot(_artInertia * Jacobian); // Inversion of projected articulated inertia @@ -1494,7 +1494,7 @@ void SingleDofJoint::updateInvProjArtInertiaImplicitDynamic( const Eigen::Matrix6d& _artInertia, double _timeStep) { // Projected articulated inertia - const Eigen::Vector6d& Jacobian = getLocalJacobianStatic(); + const Eigen::Vector6d& Jacobian = getRelativeJacobianStatic(); double projAI = Jacobian.dot(_artInertia * Jacobian); // Add additional inertia for implicit damping and spring force @@ -1558,14 +1558,14 @@ void SingleDofJoint::addChildBiasForceToDynamic( Eigen::Vector6d beta = _childBiasForce; const double coeff = getInvProjArtInertiaImplicit() * mTotalForce; beta.noalias() += _childArtInertia*(_childPartialAcc - + coeff*getLocalJacobianStatic()); + + coeff*getRelativeJacobianStatic()); // Verification assert(!math::isNan(beta)); // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(getLocalTransform(), beta); + _parentBiasForce += math::dAdInvT(getRelativeTransform(), beta); } //============================================================================== @@ -1578,14 +1578,14 @@ void SingleDofJoint::addChildBiasForceToKinematic( // Compute beta Eigen::Vector6d beta = _childBiasForce; beta.noalias() += _childArtInertia*(_childPartialAcc - + getAccelerationStatic()*getLocalJacobianStatic()); + + getAccelerationStatic()*getRelativeJacobianStatic()); // Verification assert(!math::isNan(beta)); // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(getLocalTransform(), beta); + _parentBiasForce += math::dAdInvT(getRelativeTransform(), beta); } //============================================================================== @@ -1625,7 +1625,7 @@ void SingleDofJoint::addChildBiasImpulseToDynamic( // Compute beta const Eigen::Vector6d beta = _childBiasImpulse - + _childArtInertia * getLocalJacobianStatic() + + _childArtInertia * getRelativeJacobianStatic() * getInvProjArtInertia() * mTotalImpulse; // Verification @@ -1633,7 +1633,7 @@ void SingleDofJoint::addChildBiasImpulseToDynamic( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasImpulse += math::dAdInvT(getLocalTransform(), beta); + _parentBiasImpulse += math::dAdInvT(getRelativeTransform(), beta); } //============================================================================== @@ -1644,7 +1644,7 @@ void SingleDofJoint::addChildBiasImpulseToKinematic( { // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasImpulse += math::dAdInvT(getLocalTransform(), _childBiasImpulse); + _parentBiasImpulse += math::dAdInvT(getRelativeTransform(), _childBiasImpulse); } //============================================================================== @@ -1700,7 +1700,7 @@ void SingleDofJoint::updateTotalForceDynamic( // Compute alpha mTotalForce = mAspectState.mForce + springForce + dampingForce - - getLocalJacobianStatic().dot(_bodyForce); + - getRelativeJacobianStatic().dot(_bodyForce); } //============================================================================== @@ -1735,7 +1735,7 @@ void SingleDofJoint::updateTotalImpulse(const Eigen::Vector6d& _bodyImpulse) void SingleDofJoint::updateTotalImpulseDynamic( const Eigen::Vector6d& _bodyImpulse) { - mTotalImpulse = mConstraintImpulse - getLocalJacobianStatic().dot(_bodyImpulse); + mTotalImpulse = mConstraintImpulse - getRelativeJacobianStatic().dot(_bodyImpulse); } //============================================================================== @@ -1778,8 +1778,8 @@ void SingleDofJoint::updateAccelerationDynamic( const Eigen::Matrix6d& _artInertia, const Eigen::Vector6d& _spatialAcc) { // - const double val = getLocalJacobianStatic().dot( - _artInertia * math::AdInvT(getLocalTransform(), _spatialAcc)); + const double val = getRelativeJacobianStatic().dot( + _artInertia * math::AdInvT(getRelativeTransform(), _spatialAcc)); setAccelerationStatic(getInvProjArtInertiaImplicit() * (mTotalForce - val)); // Verification @@ -1825,8 +1825,8 @@ void SingleDofJoint::updateVelocityChangeDynamic( mVelocityChange = getInvProjArtInertia() * (mTotalImpulse - - getLocalJacobianStatic().dot( - _artInertia * math::AdInvT(getLocalTransform(), _velocityChange))); + - getRelativeJacobianStatic().dot( + _artInertia * math::AdInvT(getRelativeTransform(), _velocityChange))); // Verification assert(!math::isNan(mVelocityChange)); @@ -1846,7 +1846,7 @@ void SingleDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, bool _withDampingForces, bool _withSpringForces) { - mAspectState.mForce = getLocalJacobianStatic().dot(_bodyForce); + mAspectState.mForce = getRelativeJacobianStatic().dot(_bodyForce); // Damping force if (_withDampingForces) @@ -1895,7 +1895,7 @@ void SingleDofJoint::updateForceFD(const Eigen::Vector6d& _bodyForce, //============================================================================== void SingleDofJoint::updateImpulseID(const Eigen::Vector6d& _bodyImpulse) { - mImpulse = getLocalJacobianStatic().dot(_bodyImpulse); + mImpulse = getRelativeJacobianStatic().dot(_bodyImpulse); } //============================================================================== @@ -1963,7 +1963,7 @@ void SingleDofJoint::addChildBiasForceForInvMassMatrix( { // Compute beta Eigen::Vector6d beta = _childBiasForce; - beta.noalias() += _childArtInertia * getLocalJacobianStatic() + beta.noalias() += _childArtInertia * getRelativeJacobianStatic() * getInvProjArtInertia() * mInvM_a; // Verification @@ -1971,7 +1971,7 @@ void SingleDofJoint::addChildBiasForceForInvMassMatrix( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(getLocalTransform(), beta); + _parentBiasForce += math::dAdInvT(getRelativeTransform(), beta); } //============================================================================== @@ -1982,7 +1982,7 @@ void SingleDofJoint::addChildBiasForceForInvAugMassMatrix( { // Compute beta Eigen::Vector6d beta = _childBiasForce; - beta.noalias() += _childArtInertia * getLocalJacobianStatic() + beta.noalias() += _childArtInertia * getRelativeJacobianStatic() * getInvProjArtInertiaImplicit() * mInvM_a; // Verification @@ -1990,7 +1990,7 @@ void SingleDofJoint::addChildBiasForceForInvAugMassMatrix( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(getLocalTransform(), beta); + _parentBiasForce += math::dAdInvT(getRelativeTransform(), beta); } //============================================================================== @@ -1998,7 +1998,7 @@ void SingleDofJoint::updateTotalForceForInvMassMatrix( const Eigen::Vector6d& _bodyForce) { // Compute alpha - mInvM_a = mAspectState.mForce - getLocalJacobianStatic().dot(_bodyForce); + mInvM_a = mAspectState.mForce - getRelativeJacobianStatic().dot(_bodyForce); } //============================================================================== @@ -2010,8 +2010,8 @@ void SingleDofJoint::getInvMassMatrixSegment(Eigen::MatrixXd& _invMassMat, // mInvMassMatrixSegment = getInvProjArtInertia() - * (mInvM_a - getLocalJacobianStatic().dot( - _artInertia * math::AdInvT(getLocalTransform(), _spatialAcc))); + * (mInvM_a - getRelativeJacobianStatic().dot( + _artInertia * math::AdInvT(getRelativeTransform(), _spatialAcc))); // Verification assert(!math::isNan(mInvMassMatrixSegment)); @@ -2033,8 +2033,8 @@ void SingleDofJoint::getInvAugMassMatrixSegment( // mInvMassMatrixSegment = getInvProjArtInertiaImplicit() - * (mInvM_a - getLocalJacobianStatic().dot( - _artInertia * math::AdInvT(getLocalTransform(), _spatialAcc))); + * (mInvM_a - getRelativeJacobianStatic().dot( + _artInertia * math::AdInvT(getRelativeTransform(), _spatialAcc))); // Verification assert(!math::isNan(mInvMassMatrixSegment)); @@ -2050,7 +2050,7 @@ void SingleDofJoint::getInvAugMassMatrixSegment( void SingleDofJoint::addInvMassMatrixSegmentTo(Eigen::Vector6d& _acc) { // - _acc += getLocalJacobianStatic() * mInvMassMatrixSegment; + _acc += getRelativeJacobianStatic() * mInvMassMatrixSegment; } //============================================================================== @@ -2058,7 +2058,7 @@ Eigen::VectorXd SingleDofJoint::getSpatialToGeneralized( const Eigen::Vector6d& _spatial) { Eigen::VectorXd generalized = Eigen::VectorXd::Zero(1); - generalized[0] = getLocalJacobianStatic().dot(_spatial); + generalized[0] = getRelativeJacobianStatic().dot(_spatial); return generalized; } diff --git a/dart/dynamics/SingleDofJoint.hpp b/dart/dynamics/SingleDofJoint.hpp index fa10b4c61fb5c..8e5df887c0fb9 100644 --- a/dart/dynamics/SingleDofJoint.hpp +++ b/dart/dynamics/SingleDofJoint.hpp @@ -400,14 +400,14 @@ class SingleDofJoint : public detail::SingleDofJointBase double getPotentialEnergy() const override; // Documentation inherited - const math::Jacobian getLocalJacobian() const override; + const math::Jacobian getRelativeJacobian() const override; // Documentation inherited - math::Jacobian getLocalJacobian( + math::Jacobian getRelativeJacobian( const Eigen::VectorXd& _positions) const override; // Documentation inherited - const math::Jacobian getLocalJacobianTimeDeriv() const override; + const math::Jacobian getRelativeJacobianTimeDeriv() const override; // Documentation inherited Eigen::Vector6d getBodyConstraintWrench() const override; @@ -431,19 +431,19 @@ class SingleDofJoint : public detail::SingleDofJointBase //---------------------------------------------------------------------------- // Documentation inherited - void updateLocalSpatialVelocity() const override; + void updateRelativeSpatialVelocity() const override; // Documentation inherited - void updateLocalSpatialAcceleration() const override; + void updateRelativeSpatialAcceleration() const override; // Documentation inherited - void updateLocalPrimaryAcceleration() const override; + void updateRelativePrimaryAcceleration() const override; - /// Fixed-size version of getLocalJacobian() - const Eigen::Vector6d& getLocalJacobianStatic() const; + /// Fixed-size version of getRelativeJacobian() + const Eigen::Vector6d& getRelativeJacobianStatic() const; - /// Fixed-size version of getLocalJacobianTimeDeriv() - const Eigen::Vector6d& getLocalJacobianTimeDerivStatic() const; + /// Fixed-size version of getRelativeJacobianTimeDeriv() + const Eigen::Vector6d& getRelativeJacobianTimeDerivStatic() const; /// Get the inverse of projected articulated inertia const double& getInvProjArtInertia() const; @@ -609,12 +609,12 @@ class SingleDofJoint : public detail::SingleDofJointBase /// Spatial Jacobian expressed in the child body frame /// - /// Do not use directly! Use getLocalJacobianStatic() to access this quantity + /// Do not use directly! Use getRelativeJacobianStatic() to access this quantity mutable Eigen::Vector6d mJacobian; /// Time derivative of spatial Jacobian expressed in the child body frame /// - /// Do not use directly! Use getLocalJacobianTimeDerivStatic() to access this + /// Do not use directly! Use getRelativeJacobianTimeDerivStatic() to access this /// quantity Eigen::Vector6d mJacobianDeriv; diff --git a/dart/dynamics/Skeleton.cpp b/dart/dynamics/Skeleton.cpp index 4b87c729846ed..19c941702f134 100644 --- a/dart/dynamics/Skeleton.cpp +++ b/dart/dynamics/Skeleton.cpp @@ -557,11 +557,8 @@ void Skeleton::setAspectProperties(const AspectProperties& properties) setMobile(properties.mIsMobile); setGravity(properties.mGravity); setTimeStep(properties.mTimeStep); - - if(properties.mEnabledSelfCollisionCheck) - enableSelfCollision(properties.mEnabledAdjacentBodyCheck); - else - disableSelfCollision(); + setSelfCollisionCheck(properties.mEnabledSelfCollisionCheck); + setAdjacentBodyCheck(properties.mEnabledAdjacentBodyCheck); } //============================================================================== @@ -636,31 +633,79 @@ void Skeleton::addEntryToSoftBodyNodeNameMgr(SoftBodyNode* _newNode) } //============================================================================== -void Skeleton::enableSelfCollision(bool _enableAdjacentBodyCheck) +void Skeleton::enableSelfCollision(bool enableAdjacentBodyCheck) { - mAspectProperties.mEnabledSelfCollisionCheck = true; - mAspectProperties.mEnabledAdjacentBodyCheck = _enableAdjacentBodyCheck; + enableSelfCollisionCheck(); + setAdjacentBodyCheck(enableAdjacentBodyCheck); } //============================================================================== void Skeleton::disableSelfCollision() { - mAspectProperties.mEnabledSelfCollisionCheck = false; - mAspectProperties.mEnabledAdjacentBodyCheck = false; + disableSelfCollisionCheck(); + setAdjacentBodyCheck(false); } //============================================================================== -bool Skeleton::isEnabledSelfCollisionCheck() const +void Skeleton::setSelfCollisionCheck(bool enable) +{ + mAspectProperties.mEnabledSelfCollisionCheck = enable; +} + +//============================================================================== +bool Skeleton::getSelfCollisionCheck() const { return mAspectProperties.mEnabledSelfCollisionCheck; } //============================================================================== -bool Skeleton::isEnabledAdjacentBodyCheck() const +void Skeleton::enableSelfCollisionCheck() +{ + setSelfCollisionCheck(true); +} + +//============================================================================== +void Skeleton::disableSelfCollisionCheck() +{ + setSelfCollisionCheck(false); +} + +//============================================================================== +bool Skeleton::isEnabledSelfCollisionCheck() const +{ + return getSelfCollisionCheck(); +} + +//============================================================================== +void Skeleton::setAdjacentBodyCheck(bool enable) +{ + mAspectProperties.mEnabledAdjacentBodyCheck = enable; +} + +//============================================================================== +bool Skeleton::getAdjacentBodyCheck() const { return mAspectProperties.mEnabledAdjacentBodyCheck; } +//============================================================================== +void Skeleton::enableAdjacentBodyCheck() +{ + setAdjacentBodyCheck(true); +} + +//============================================================================== +void Skeleton::disableAdjacentBodyCheck() +{ + setAdjacentBodyCheck(false); +} + +//============================================================================== +bool Skeleton::isEnabledAdjacentBodyCheck() const +{ + return getAdjacentBodyCheck(); +} + //============================================================================== void Skeleton::setMobile(bool _isMobile) { diff --git a/dart/dynamics/Skeleton.hpp b/dart/dynamics/Skeleton.hpp index 96661a09741d0..0fff96313b3e3 100644 --- a/dart/dynamics/Skeleton.hpp +++ b/dart/dynamics/Skeleton.hpp @@ -226,17 +226,46 @@ class Skeleton : /// Get name. const std::string& getName() const override; - /// Enable self collision check - void enableSelfCollision(bool _enableAdjacentBodyCheck = false); + /// Deprecated. Please use enableSelfCollision() and setAdjacentBodyCheck() + /// instead. + DEPRECATED(6.0) + void enableSelfCollision(bool enableAdjacentBodyCheck = false); - /// Disable self collision check + /// Deprecated. Please use disableSelfCollisionCheck() instead. + DEPRECATED(6.0) void disableSelfCollision(); - /// Return true if self collision check is enabled + /// Set whether to check self-collision. + void setSelfCollisionCheck(bool enable); + + /// Return whether self-collision check is enabled. + bool getSelfCollisionCheck() const; + + /// Enable self-collision check. + void enableSelfCollisionCheck(); + + /// Disable self-collision check. + void disableSelfCollisionCheck(); + + /// Return true if self-collision check is enabled bool isEnabledSelfCollisionCheck() const; - /// Return true if self collision check is enabled including adjacent - /// bodies + /// Set whether to check adjacent bodies. This option is effective only when + /// the self-collision check is enabled. + void setAdjacentBodyCheck(bool enable); + + /// Return whether adjacent body check is enabled. + bool getAdjacentBodyCheck() const; + + /// Enable collision check for adjacent bodies. This option is effective only + /// when the self-collision check is enabled. + void enableAdjacentBodyCheck(); + + /// Disable collision check for adjacent bodies. This option is effective only + /// when the self-collision check is enabled. + void disableAdjacentBodyCheck(); + + /// Return true if self-collision check is enabled including adjacent bodies. bool isEnabledAdjacentBodyCheck() const; /// Set whether this skeleton will be updated by forward dynamics. diff --git a/dart/dynamics/SoftBodyNode.cpp b/dart/dynamics/SoftBodyNode.cpp index 5affc46e83e2b..2a1516526f4ee 100644 --- a/dart/dynamics/SoftBodyNode.cpp +++ b/dart/dynamics/SoftBodyNode.cpp @@ -616,7 +616,7 @@ void SoftBodyNode::updateTransmittedForceID(const Eigen::Vector3d& _gravity, Joint* childJoint = childBodyNode->getParentJoint(); assert(childJoint != nullptr); - mF += math::dAdInvT(childJoint->getLocalTransform(), + mF += math::dAdInvT(childJoint->getRelativeTransform(), childBodyNode->getBodyForce()); } for (auto& pointMass : mPointMasses) @@ -862,7 +862,7 @@ void SoftBodyNode::aggregateMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col) // for (std::vector::const_iterator it = mChildBodyNodes.begin(); // it != mChildBodyNodes.end(); ++it) // { -// mM_F += math::dAdInvT((*it)->getParentJoint()->getLocalTransform(), +// mM_F += math::dAdInvT((*it)->getParentJoint()->getRelativeTransform(), // (*it)->mM_F); // } @@ -883,7 +883,7 @@ void SoftBodyNode::aggregateMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _col) // { // int iStart = mParentJoint->getIndexInTree(0); // _MCol->block(iStart, _col, dof, 1).noalias() -// = mParentJoint->getLocalJacobian().transpose() * mM_F; +// = mParentJoint->getRelativeJacobian().transpose() * mM_F; // } } @@ -906,7 +906,7 @@ void SoftBodyNode::aggregateAugMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _c for (std::vector::const_iterator it = mChildBodyNodes.begin(); it != mChildBodyNodes.end(); ++it) { - mM_F += math::dAdInvT((*it)->getParentJoint()->getLocalTransform(), + mM_F += math::dAdInvT((*it)->getParentJoint()->getRelativeTransform(), (*it)->mM_F); } for (std::vector::iterator it = mPointMasses.begin(); @@ -931,7 +931,7 @@ void SoftBodyNode::aggregateAugMassMatrix(Eigen::MatrixXd& _MCol, std::size_t _c // TODO(JS): Not recommended to use Joint::getAccelerations _MCol.block(iStart, _col, dof, 1).noalias() - = mParentJoint->getLocalJacobian().transpose() * mM_F + = mParentJoint->getRelativeJacobian().transpose() * mM_F + D * (_timeStep * mParentJoint->getAccelerations()) + K * (_timeStep * _timeStep * mParentJoint->getAccelerations()); } @@ -1017,7 +1017,7 @@ void SoftBodyNode::aggregateInvMassMatrix(Eigen::MatrixXd& _InvMCol, std::size_t _InvMCol, _col, getArticulatedInertia(), mParentBodyNode->mInvM_U); // - mInvM_U = math::AdInvT(mParentJoint->getLocalTransform(), + mInvM_U = math::AdInvT(mParentJoint->getRelativeTransform(), mParentBodyNode->mInvM_U); } else @@ -1097,7 +1097,7 @@ void SoftBodyNode::aggregateGravityForceVector(Eigen::VectorXd& _g, for (std::vector::const_iterator it = mChildBodyNodes.begin(); it != mChildBodyNodes.end(); ++it) { - mG_F += math::dAdInvT((*it)->mParentJoint->getLocalTransform(), + mG_F += math::dAdInvT((*it)->mParentJoint->getRelativeTransform(), (*it)->mG_F); } @@ -1111,7 +1111,7 @@ void SoftBodyNode::aggregateGravityForceVector(Eigen::VectorXd& _g, int nGenCoords = mParentJoint->getNumDofs(); if (nGenCoords > 0) { - Eigen::VectorXd g = -(mParentJoint->getLocalJacobian().transpose() * mG_F); + Eigen::VectorXd g = -(mParentJoint->getRelativeJacobian().transpose() * mG_F); int iStart = mParentJoint->getIndexInTree(0); _g.segment(iStart, nGenCoords) = g; } @@ -1150,7 +1150,7 @@ void SoftBodyNode::aggregateCombinedVector(Eigen::VectorXd& _Cg, // for (std::vector::iterator it = mChildBodyNodes.begin(); // it != mChildBodyNodes.end(); ++it) // { -// mCg_F += math::dAdInvT((*it)->getParentJoint()->getLocalTransform(), +// mCg_F += math::dAdInvT((*it)->getParentJoint()->getRelativeTransform(), // (*it)->mCg_F); // } @@ -1164,7 +1164,7 @@ void SoftBodyNode::aggregateCombinedVector(Eigen::VectorXd& _Cg, // int nGenCoords = mParentJoint->getNumDofs(); // if (nGenCoords > 0) // { -// Eigen::VectorXd Cg = mParentJoint->getLocalJacobian().transpose() * mCg_F; +// Eigen::VectorXd Cg = mParentJoint->getRelativeJacobian().transpose() * mCg_F; // int iStart = mParentJoint->getIndexInTree(0); // _Cg->segment(iStart, nGenCoords) = Cg; // } @@ -1183,7 +1183,7 @@ void SoftBodyNode::aggregateExternalForces(Eigen::VectorXd& _Fext) for (std::vector::const_iterator it = mChildBodyNodes.begin(); it != mChildBodyNodes.end(); ++it) { - mFext_F += math::dAdInvT((*it)->mParentJoint->getLocalTransform(), + mFext_F += math::dAdInvT((*it)->mParentJoint->getRelativeTransform(), (*it)->mFext_F); } @@ -1198,7 +1198,7 @@ void SoftBodyNode::aggregateExternalForces(Eigen::VectorXd& _Fext) if (nGenCoords > 0) { Eigen::VectorXd Fext - = mParentJoint->getLocalJacobian().transpose() * mFext_F; + = mParentJoint->getRelativeJacobian().transpose() * mFext_F; int iStart = mParentJoint->getIndexInTree(0); _Fext.segment(iStart, nGenCoords) = Fext; } diff --git a/dart/dynamics/TranslationalJoint.cpp b/dart/dynamics/TranslationalJoint.cpp index c84a34cc04611..99d4d72d9a6bc 100644 --- a/dart/dynamics/TranslationalJoint.cpp +++ b/dart/dynamics/TranslationalJoint.cpp @@ -59,11 +59,11 @@ TranslationalJoint::~TranslationalJoint() } //============================================================================== -Eigen::Matrix TranslationalJoint::getLocalJacobianStatic( +Eigen::Matrix TranslationalJoint::getRelativeJacobianStatic( const Eigen::Vector3d& /*_positions*/) const { // The Jacobian is always constant w.r.t. the generalized coordinates. - return getLocalJacobianStatic(); + return getRelativeJacobianStatic(); } //============================================================================== @@ -119,7 +119,7 @@ void TranslationalJoint::updateDegreeOfFreedomNames() } //============================================================================== -void TranslationalJoint::updateLocalTransform() const +void TranslationalJoint::updateRelativeTransform() const { mT = Joint::mAspectProperties.mT_ParentBodyToJoint * Eigen::Translation3d(getPositionsStatic()) @@ -130,7 +130,7 @@ void TranslationalJoint::updateLocalTransform() const } //============================================================================== -void TranslationalJoint::updateLocalJacobian(bool _mandatory) const +void TranslationalJoint::updateRelativeJacobian(bool _mandatory) const { if (_mandatory) { @@ -143,7 +143,7 @@ void TranslationalJoint::updateLocalJacobian(bool _mandatory) const } //============================================================================== -void TranslationalJoint::updateLocalJacobianTimeDeriv() const +void TranslationalJoint::updateRelativeJacobianTimeDeriv() const { // Time derivative of translational joint is always zero assert(mJacobianDeriv == (Eigen::Matrix::Zero())); diff --git a/dart/dynamics/TranslationalJoint.hpp b/dart/dynamics/TranslationalJoint.hpp index 7a537572d6c4d..c39663bb5e468 100644 --- a/dart/dynamics/TranslationalJoint.hpp +++ b/dart/dynamics/TranslationalJoint.hpp @@ -76,7 +76,7 @@ class TranslationalJoint : public MultiDofJoint<3> // Documentation inherited bool isCyclic(std::size_t _index) const override; - Eigen::Matrix getLocalJacobianStatic( + Eigen::Matrix getRelativeJacobianStatic( const Eigen::Vector3d& _positions) const override; protected: @@ -87,19 +87,19 @@ class TranslationalJoint : public MultiDofJoint<3> // Documentation inherited Joint* clone() const override; - using MultiDofJoint::getLocalJacobianStatic; + using MultiDofJoint::getRelativeJacobianStatic; // Documentation inherited void updateDegreeOfFreedomNames() override; // Documentation inherited - void updateLocalTransform() const override; + void updateRelativeTransform() const override; // Documentation inherited - void updateLocalJacobian(bool _mandatory = true) const override; + void updateRelativeJacobian(bool _mandatory = true) const override; // Documentation inherited - void updateLocalJacobianTimeDeriv() const override; + void updateRelativeJacobianTimeDeriv() const override; public: // To get byte-aligned Eigen vectors diff --git a/dart/dynamics/UniversalJoint.cpp b/dart/dynamics/UniversalJoint.cpp index 7786bf0151ca6..bb3421079d12a 100644 --- a/dart/dynamics/UniversalJoint.cpp +++ b/dart/dynamics/UniversalJoint.cpp @@ -150,7 +150,7 @@ const Eigen::Vector3d& UniversalJoint::getAxis2() const } //============================================================================== -Eigen::Matrix UniversalJoint::getLocalJacobianStatic( +Eigen::Matrix UniversalJoint::getRelativeJacobianStatic( const Eigen::Vector2d& _positions) const { Eigen::Matrix J; @@ -189,7 +189,7 @@ void UniversalJoint::updateDegreeOfFreedomNames() } //============================================================================== -void UniversalJoint::updateLocalTransform() const +void UniversalJoint::updateRelativeTransform() const { const Eigen::Vector2d& positions = getPositionsStatic(); mT = Joint::mAspectProperties.mT_ParentBodyToJoint @@ -200,15 +200,15 @@ void UniversalJoint::updateLocalTransform() const } //============================================================================== -void UniversalJoint::updateLocalJacobian(bool) const +void UniversalJoint::updateRelativeJacobian(bool) const { - mJacobian = getLocalJacobianStatic(getPositionsStatic()); + mJacobian = getRelativeJacobianStatic(getPositionsStatic()); } //============================================================================== -void UniversalJoint::updateLocalJacobianTimeDeriv() const +void UniversalJoint::updateRelativeJacobianTimeDeriv() const { - Eigen::Vector6d tmpV1 = getLocalJacobianStatic().col(1) + Eigen::Vector6d tmpV1 = getRelativeJacobianStatic().col(1) * getVelocitiesStatic()[1]; Eigen::Isometry3d tmpT = math::expAngular( diff --git a/dart/dynamics/UniversalJoint.hpp b/dart/dynamics/UniversalJoint.hpp index 9351f035c2252..abc73c209dc62 100644 --- a/dart/dynamics/UniversalJoint.hpp +++ b/dart/dynamics/UniversalJoint.hpp @@ -102,7 +102,7 @@ class UniversalJoint : public detail::UniversalJointBase const Eigen::Vector3d& getAxis2() const; // Documentation inherited - Eigen::Matrix getLocalJacobianStatic( + Eigen::Matrix getRelativeJacobianStatic( const Eigen::Vector2d& _positions) const override; protected: @@ -113,19 +113,19 @@ class UniversalJoint : public detail::UniversalJointBase // Documentation inherited Joint* clone() const override; - using MultiDofJoint::getLocalJacobianStatic; + using MultiDofJoint::getRelativeJacobianStatic; // Documentation inherited void updateDegreeOfFreedomNames() override; // Documentation inherited - void updateLocalTransform() const override; + void updateRelativeTransform() const override; // Documentation inherited - void updateLocalJacobian(bool =true) const override; + void updateRelativeJacobian(bool =true) const override; // Documentation inherited - void updateLocalJacobianTimeDeriv() const override; + void updateRelativeJacobianTimeDeriv() const override; public: // To get byte-aligned Eigen vectors diff --git a/dart/dynamics/WeldJoint.cpp b/dart/dynamics/WeldJoint.cpp index 42b25a024f2c3..caaaf8eebec68 100644 --- a/dart/dynamics/WeldJoint.cpp +++ b/dart/dynamics/WeldJoint.cpp @@ -113,39 +113,39 @@ Joint* WeldJoint::clone() const } //============================================================================== -void WeldJoint::updateLocalTransform() const +void WeldJoint::updateRelativeTransform() const { // Do nothing } //============================================================================== -void WeldJoint::updateLocalSpatialVelocity() const +void WeldJoint::updateRelativeSpatialVelocity() const { // Do nothing // Should we have mSpatialVelocity.setZero() here instead? } //============================================================================== -void WeldJoint::updateLocalSpatialAcceleration() const +void WeldJoint::updateRelativeSpatialAcceleration() const { // Do nothing // Should we have mSpatialAcceleration.setZero() here instead? } //============================================================================== -void WeldJoint::updateLocalPrimaryAcceleration() const +void WeldJoint::updateRelativePrimaryAcceleration() const { // Do nothing } //============================================================================== -void WeldJoint::updateLocalJacobian(bool) const +void WeldJoint::updateRelativeJacobian(bool) const { // Do nothing } //============================================================================== -void WeldJoint::updateLocalJacobianTimeDeriv() const +void WeldJoint::updateRelativeJacobianTimeDeriv() const { // Do nothing } diff --git a/dart/dynamics/WeldJoint.hpp b/dart/dynamics/WeldJoint.hpp index 46bdaa14b366f..86c15026c3e35 100644 --- a/dart/dynamics/WeldJoint.hpp +++ b/dart/dynamics/WeldJoint.hpp @@ -95,22 +95,22 @@ class WeldJoint : public ZeroDofJoint //---------------------------------------------------------------------------- // Documentation inherited - void updateLocalTransform() const override; + void updateRelativeTransform() const override; // Documentation inherited - void updateLocalSpatialVelocity() const override; + void updateRelativeSpatialVelocity() const override; // Documentation inherited - void updateLocalSpatialAcceleration() const override; + void updateRelativeSpatialAcceleration() const override; // Documentation inherited - void updateLocalPrimaryAcceleration() const override; + void updateRelativePrimaryAcceleration() const override; // Documentation inherited - void updateLocalJacobian(bool =true) const override; + void updateRelativeJacobian(bool =true) const override; // Documentation inherited - void updateLocalJacobianTimeDeriv() const override; + void updateRelativeJacobianTimeDeriv() const override; public: // To get byte-aligned Eigen vectors diff --git a/dart/dynamics/ZeroDofJoint.cpp b/dart/dynamics/ZeroDofJoint.cpp index b237ff4f8d70e..26cf32a95379e 100644 --- a/dart/dynamics/ZeroDofJoint.cpp +++ b/dart/dynamics/ZeroDofJoint.cpp @@ -592,20 +592,20 @@ Eigen::Vector6d ZeroDofJoint::getBodyConstraintWrench() const } //============================================================================== -const math::Jacobian ZeroDofJoint::getLocalJacobian() const +const math::Jacobian ZeroDofJoint::getRelativeJacobian() const { return Eigen::Matrix(); } //============================================================================== -math::Jacobian ZeroDofJoint::getLocalJacobian( +math::Jacobian ZeroDofJoint::getRelativeJacobian( const Eigen::VectorXd& /*_positions*/) const { return Eigen::Matrix(); } //============================================================================== -const math::Jacobian ZeroDofJoint::getLocalJacobianTimeDeriv() const +const math::Jacobian ZeroDofJoint::getRelativeJacobianTimeDeriv() const { return Eigen::Matrix(); } @@ -643,7 +643,7 @@ void ZeroDofJoint::addChildArtInertiaTo( { // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. - _parentArtInertia += math::transformInertia(getLocalTransform().inverse(), + _parentArtInertia += math::transformInertia(getRelativeTransform().inverse(), _childArtInertia); } @@ -653,7 +653,7 @@ void ZeroDofJoint::addChildArtInertiaImplicitTo( { // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. - _parentArtInertia += math::transformInertia(getLocalTransform().inverse(), + _parentArtInertia += math::transformInertia(getRelativeTransform().inverse(), _childArtInertia); } @@ -681,7 +681,7 @@ void ZeroDofJoint::addChildBiasForceTo( { // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(getLocalTransform(), _childBiasForce + _parentBiasForce += math::dAdInvT(getRelativeTransform(), _childBiasForce + _childArtInertia*_childPartialAcc); } @@ -693,7 +693,7 @@ void ZeroDofJoint::addChildBiasImpulseTo( { // Add child body's bias force to parent body's bias impulse. Note that mT // should be updated. - _parentBiasImpulse += math::dAdInvT(getLocalTransform(), _childBiasImpulse); + _parentBiasImpulse += math::dAdInvT(getRelativeTransform(), _childBiasImpulse); } //============================================================================== diff --git a/dart/dynamics/ZeroDofJoint.hpp b/dart/dynamics/ZeroDofJoint.hpp index fa209ae00732c..f1b175ce242c6 100644 --- a/dart/dynamics/ZeroDofJoint.hpp +++ b/dart/dynamics/ZeroDofJoint.hpp @@ -366,14 +366,14 @@ class ZeroDofJoint : public Joint //---------------------------------------------------------------------------- // Documentation inherited - const math::Jacobian getLocalJacobian() const override; + const math::Jacobian getRelativeJacobian() const override; // Documentation inherited - math::Jacobian getLocalJacobian( + math::Jacobian getRelativeJacobian( const Eigen::VectorXd& _positions) const override; // Documentation inherited - const math::Jacobian getLocalJacobianTimeDeriv() const override; + const math::Jacobian getRelativeJacobianTimeDeriv() const override; // Documentation inherited void addVelocityTo(Eigen::Vector6d& _vel) override; diff --git a/dart/dynamics/detail/MultiDofJoint.hpp b/dart/dynamics/detail/MultiDofJoint.hpp index 23a73c442c2d5..dcfaa98cd3a84 100644 --- a/dart/dynamics/detail/MultiDofJoint.hpp +++ b/dart/dynamics/detail/MultiDofJoint.hpp @@ -1343,7 +1343,7 @@ Eigen::Vector6d MultiDofJoint::getBodyConstraintWrench() const { assert(this->mChildBodyNode); return this->mChildBodyNode->getBodyForce() - - this->getLocalJacobianStatic() * this->mAspectState.mForces; + - this->getRelativeJacobianStatic() * this->mAspectState.mForces; } //============================================================================== @@ -1381,40 +1381,40 @@ void MultiDofJoint::registerDofs() //============================================================================== template -const math::Jacobian MultiDofJoint::getLocalJacobian() const +const math::Jacobian MultiDofJoint::getRelativeJacobian() const { - return getLocalJacobianStatic(); + return getRelativeJacobianStatic(); } //============================================================================== template const Eigen::Matrix& -MultiDofJoint::getLocalJacobianStatic() const +MultiDofJoint::getRelativeJacobianStatic() const { - if(this->mIsLocalJacobianDirty) + if(this->mIsRelativeJacobianDirty) { - this->updateLocalJacobian(false); - this->mIsLocalJacobianDirty = false; + this->updateRelativeJacobian(false); + this->mIsRelativeJacobianDirty = false; } return mJacobian; } //============================================================================== template -math::Jacobian MultiDofJoint::getLocalJacobian( +math::Jacobian MultiDofJoint::getRelativeJacobian( const Eigen::VectorXd& _positions) const { - return getLocalJacobianStatic(_positions); + return getRelativeJacobianStatic(_positions); } //============================================================================== template -const math::Jacobian MultiDofJoint::getLocalJacobianTimeDeriv() const +const math::Jacobian MultiDofJoint::getRelativeJacobianTimeDeriv() const { - if(this->mIsLocalJacobianTimeDerivDirty) + if(this->mIsRelativeJacobianTimeDerivDirty) { - this->updateLocalJacobianTimeDeriv(); - this->mIsLocalJacobianTimeDerivDirty = false; + this->updateRelativeJacobianTimeDeriv(); + this->mIsRelativeJacobianTimeDerivDirty = false; } return mJacobianDeriv; } @@ -1422,12 +1422,12 @@ const math::Jacobian MultiDofJoint::getLocalJacobianTimeDeriv() const //============================================================================== template const Eigen::Matrix& -MultiDofJoint::getLocalJacobianTimeDerivStatic() const +MultiDofJoint::getRelativeJacobianTimeDerivStatic() const { - if(this->mIsLocalJacobianTimeDerivDirty) + if(this->mIsRelativeJacobianTimeDerivDirty) { - this->updateLocalJacobianTimeDeriv(); - this->mIsLocalJacobianTimeDerivDirty = false; + this->updateRelativeJacobianTimeDeriv(); + this->mIsRelativeJacobianTimeDerivDirty = false; } return mJacobianDeriv; } @@ -1452,27 +1452,27 @@ MultiDofJoint::getInvProjArtInertiaImplicit() const //============================================================================== template -void MultiDofJoint::updateLocalSpatialVelocity() const +void MultiDofJoint::updateRelativeSpatialVelocity() const { this->mSpatialVelocity = - this->getLocalJacobianStatic() * this->getVelocitiesStatic(); + this->getRelativeJacobianStatic() * this->getVelocitiesStatic(); } //============================================================================== template -void MultiDofJoint::updateLocalSpatialAcceleration() const +void MultiDofJoint::updateRelativeSpatialAcceleration() const { this->mSpatialAcceleration = - this->getLocalPrimaryAcceleration() - + this->getLocalJacobianTimeDerivStatic() * this->getVelocitiesStatic(); + this->getRelativePrimaryAcceleration() + + this->getRelativeJacobianTimeDerivStatic() * this->getVelocitiesStatic(); } //============================================================================== template -void MultiDofJoint::updateLocalPrimaryAcceleration() const +void MultiDofJoint::updateRelativePrimaryAcceleration() const { this->mPrimaryAcceleration = - this->getLocalJacobianStatic() * this->getAccelerationsStatic(); + this->getRelativeJacobianStatic() * this->getAccelerationsStatic(); } //============================================================================== @@ -1480,7 +1480,7 @@ template void MultiDofJoint::addVelocityTo(Eigen::Vector6d& _vel) { // Add joint velocity to _vel - _vel.noalias() += getLocalJacobianStatic() * getVelocitiesStatic(); + _vel.noalias() += getRelativeJacobianStatic() * getVelocitiesStatic(); // Verification assert(!math::isNan(_vel)); @@ -1494,8 +1494,8 @@ void MultiDofJoint::setPartialAccelerationTo( { // ad(V, S * dq) + dS * dq _partialAcceleration = math::ad(_childVelocity, - getLocalJacobianStatic() * getVelocitiesStatic()) - + getLocalJacobianTimeDerivStatic() * getVelocitiesStatic(); + getRelativeJacobianStatic() * getVelocitiesStatic()) + + getRelativeJacobianTimeDerivStatic() * getVelocitiesStatic(); // Verification assert(!math::isNan(_partialAcceleration)); } @@ -1505,7 +1505,7 @@ template void MultiDofJoint::addAccelerationTo(Eigen::Vector6d& _acc) { // Add joint acceleration to _acc - _acc.noalias() += getLocalJacobianStatic() * getAccelerationsStatic(); + _acc.noalias() += getRelativeJacobianStatic() * getAccelerationsStatic(); // Verification assert(!math::isNan(_acc)); @@ -1516,7 +1516,7 @@ template void MultiDofJoint::addVelocityChangeTo(Eigen::Vector6d& _velocityChange) { // Add joint velocity change to _velocityChange - _velocityChange.noalias() += getLocalJacobianStatic() * mVelocityChanges; + _velocityChange.noalias() += getRelativeJacobianStatic() * mVelocityChanges; // Verification assert(!math::isNan(_velocityChange)); @@ -1555,7 +1555,7 @@ void MultiDofJoint::addChildArtInertiaToDynamic( const Eigen::Matrix6d& _childArtInertia) { // Child body's articulated inertia - Eigen::Matrix AIS = _childArtInertia * getLocalJacobianStatic(); + Eigen::Matrix AIS = _childArtInertia * getRelativeJacobianStatic(); Eigen::Matrix6d PI = _childArtInertia; PI.noalias() -= AIS * mInvProjArtInertia * AIS.transpose(); assert(!math::isNan(PI)); @@ -1563,7 +1563,7 @@ void MultiDofJoint::addChildArtInertiaToDynamic( // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. _parentArtInertia += math::transformInertia( - this->getLocalTransform().inverse(), PI); + this->getRelativeTransform().inverse(), PI); } //============================================================================== @@ -1575,7 +1575,7 @@ void MultiDofJoint::addChildArtInertiaToKinematic( // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. _parentArtInertia += math::transformInertia( - this->getLocalTransform().inverse(), _childArtInertia); + this->getRelativeTransform().inverse(), _childArtInertia); } //============================================================================== @@ -1611,7 +1611,7 @@ void MultiDofJoint::addChildArtInertiaImplicitToDynamic( const Eigen::Matrix6d& _childArtInertia) { // Child body's articulated inertia - Eigen::Matrix AIS = _childArtInertia * getLocalJacobianStatic(); + Eigen::Matrix AIS = _childArtInertia * getRelativeJacobianStatic(); Eigen::Matrix6d PI = _childArtInertia; PI.noalias() -= AIS * mInvProjArtInertiaImplicit * AIS.transpose(); assert(!math::isNan(PI)); @@ -1619,7 +1619,7 @@ void MultiDofJoint::addChildArtInertiaImplicitToDynamic( // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. _parentArtInertia += math::transformInertia( - this->getLocalTransform().inverse(), PI); + this->getRelativeTransform().inverse(), PI); } //============================================================================== @@ -1631,7 +1631,7 @@ void MultiDofJoint::addChildArtInertiaImplicitToKinematic( // Add child body's articulated inertia to parent body's articulated inertia. // Note that mT should be updated. _parentArtInertia += math::transformInertia( - this->getLocalTransform().inverse(), _childArtInertia); + this->getRelativeTransform().inverse(), _childArtInertia); } //============================================================================== @@ -1663,7 +1663,7 @@ void MultiDofJoint::updateInvProjArtInertiaDynamic( const Eigen::Matrix6d& _artInertia) { // Projected articulated inertia - const Eigen::Matrix& Jacobian = getLocalJacobianStatic(); + const Eigen::Matrix& Jacobian = getRelativeJacobianStatic(); const Eigen::Matrix projAI = Jacobian.transpose() * _artInertia * Jacobian; @@ -1716,7 +1716,7 @@ void MultiDofJoint::updateInvProjArtInertiaImplicitDynamic( double _timeStep) { // Projected articulated inertia - const Eigen::Matrix& Jacobian = getLocalJacobianStatic(); + const Eigen::Matrix& Jacobian = getRelativeJacobianStatic(); Eigen::Matrix projAI = Jacobian.transpose() * _artInertia * Jacobian; @@ -1790,7 +1790,7 @@ void MultiDofJoint::addChildBiasForceToDynamic( = _childBiasForce + _childArtInertia * (_childPartialAcc - + getLocalJacobianStatic()*getInvProjArtInertiaImplicit() + + getRelativeJacobianStatic()*getInvProjArtInertiaImplicit() *mTotalForce); // Eigen::Vector6d beta @@ -1803,7 +1803,7 @@ void MultiDofJoint::addChildBiasForceToDynamic( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(this->getLocalTransform(), beta); + _parentBiasForce += math::dAdInvT(this->getRelativeTransform(), beta); } //============================================================================== @@ -1818,7 +1818,7 @@ void MultiDofJoint::addChildBiasForceToKinematic( const Eigen::Vector6d beta = _childBiasForce + _childArtInertia*(_childPartialAcc - + getLocalJacobianStatic()*getAccelerationsStatic()); + + getRelativeJacobianStatic()*getAccelerationsStatic()); // Eigen::Vector6d beta // = _childBiasForce; @@ -1830,7 +1830,7 @@ void MultiDofJoint::addChildBiasForceToKinematic( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(this->getLocalTransform(), beta); + _parentBiasForce += math::dAdInvT(this->getRelativeTransform(), beta); } //============================================================================== @@ -1872,7 +1872,7 @@ void MultiDofJoint::addChildBiasImpulseToDynamic( // Compute beta const Eigen::Vector6d beta = _childBiasImpulse - + _childArtInertia*getLocalJacobianStatic() + + _childArtInertia*getRelativeJacobianStatic() *getInvProjArtInertia()*mTotalImpulse; // Verification @@ -1880,7 +1880,7 @@ void MultiDofJoint::addChildBiasImpulseToDynamic( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasImpulse += math::dAdInvT(this->getLocalTransform(), beta); + _parentBiasImpulse += math::dAdInvT(this->getRelativeTransform(), beta); } //============================================================================== @@ -1893,7 +1893,7 @@ void MultiDofJoint::addChildBiasImpulseToKinematic( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. _parentBiasImpulse += math::dAdInvT( - this->getLocalTransform(), _childBiasImpulse); + this->getRelativeTransform(), _childBiasImpulse); } //============================================================================== @@ -1953,7 +1953,7 @@ void MultiDofJoint::updateTotalForceDynamic( // mTotalForce = this->mAspectState.mForces + springForce + dampingForce; - mTotalForce.noalias() -= getLocalJacobianStatic().transpose()*_bodyForce; + mTotalForce.noalias() -= getRelativeJacobianStatic().transpose()*_bodyForce; } //============================================================================== @@ -1995,7 +1995,7 @@ void MultiDofJoint::updateTotalImpulseDynamic( { // mTotalImpulse = mConstraintImpulses; - mTotalImpulse.noalias() -= getLocalJacobianStatic().transpose()*_bodyImpulse; + mTotalImpulse.noalias() -= getRelativeJacobianStatic().transpose()*_bodyImpulse; } //============================================================================== @@ -2045,8 +2045,8 @@ void MultiDofJoint::updateAccelerationDynamic( { // setAccelerationsStatic( getInvProjArtInertiaImplicit() - * (mTotalForce - getLocalJacobianStatic().transpose() - *_artInertia*math::AdInvT(this->getLocalTransform(), _spatialAcc)) ); + * (mTotalForce - getRelativeJacobianStatic().transpose() + *_artInertia*math::AdInvT(this->getRelativeTransform(), _spatialAcc)) ); // Verification assert(!math::isNan(getAccelerationsStatic())); @@ -2094,8 +2094,8 @@ void MultiDofJoint::updateVelocityChangeDynamic( // mVelocityChanges = getInvProjArtInertia() - * (mTotalImpulse - getLocalJacobianStatic().transpose() - *_artInertia*math::AdInvT(this->getLocalTransform(), _velocityChange)); + * (mTotalImpulse - getRelativeJacobianStatic().transpose() + *_artInertia*math::AdInvT(this->getRelativeTransform(), _velocityChange)); // Verification assert(!math::isNan(mVelocityChanges)); @@ -2117,7 +2117,7 @@ void MultiDofJoint::updateForceID(const Eigen::Vector6d& _bodyForce, bool _withDampingForces, bool _withSpringForces) { - this->mAspectState.mForces = getLocalJacobianStatic().transpose()*_bodyForce; + this->mAspectState.mForces = getRelativeJacobianStatic().transpose()*_bodyForce; // Damping force if (_withDampingForces) @@ -2168,7 +2168,7 @@ void MultiDofJoint::updateForceFD(const Eigen::Vector6d& _bodyForce, template void MultiDofJoint::updateImpulseID(const Eigen::Vector6d& _bodyImpulse) { - mImpulses = getLocalJacobianStatic().transpose()*_bodyImpulse; + mImpulses = getRelativeJacobianStatic().transpose()*_bodyImpulse; } //============================================================================== @@ -2244,7 +2244,7 @@ void MultiDofJoint::addChildBiasForceForInvMassMatrix( { // Compute beta Eigen::Vector6d beta = _childBiasForce; - beta.noalias() += _childArtInertia * getLocalJacobianStatic() + beta.noalias() += _childArtInertia * getRelativeJacobianStatic() * getInvProjArtInertia() * mInvM_a; // Verification @@ -2252,7 +2252,7 @@ void MultiDofJoint::addChildBiasForceForInvMassMatrix( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(this->getLocalTransform(), beta); + _parentBiasForce += math::dAdInvT(this->getRelativeTransform(), beta); } //============================================================================== @@ -2264,7 +2264,7 @@ void MultiDofJoint::addChildBiasForceForInvAugMassMatrix( { // Compute beta Eigen::Vector6d beta = _childBiasForce; - beta.noalias() += _childArtInertia * getLocalJacobianStatic() + beta.noalias() += _childArtInertia * getRelativeJacobianStatic() * getInvProjArtInertiaImplicit() * mInvM_a; // Verification @@ -2272,7 +2272,7 @@ void MultiDofJoint::addChildBiasForceForInvAugMassMatrix( // Add child body's bias force to parent body's bias force. Note that mT // should be updated. - _parentBiasForce += math::dAdInvT(this->getLocalTransform(), beta); + _parentBiasForce += math::dAdInvT(this->getRelativeTransform(), beta); } //============================================================================== @@ -2282,7 +2282,7 @@ void MultiDofJoint::updateTotalForceForInvMassMatrix( { // Compute alpha mInvM_a = this->mAspectState.mForces; - mInvM_a.noalias() -= getLocalJacobianStatic().transpose() * _bodyForce; + mInvM_a.noalias() -= getRelativeJacobianStatic().transpose() * _bodyForce; } //============================================================================== @@ -2296,8 +2296,8 @@ void MultiDofJoint::getInvMassMatrixSegment( // mInvMassMatrixSegment = getInvProjArtInertia() - * (mInvM_a - getLocalJacobianStatic().transpose() - * _artInertia * math::AdInvT(this->getLocalTransform(), _spatialAcc)); + * (mInvM_a - getRelativeJacobianStatic().transpose() + * _artInertia * math::AdInvT(this->getRelativeTransform(), _spatialAcc)); // Verification assert(!math::isNan(mInvMassMatrixSegment)); @@ -2320,8 +2320,8 @@ void MultiDofJoint::getInvAugMassMatrixSegment( // mInvMassMatrixSegment = getInvProjArtInertiaImplicit() - * (mInvM_a - getLocalJacobianStatic().transpose() - * _artInertia * math::AdInvT(this->getLocalTransform(), _spatialAcc)); + * (mInvM_a - getRelativeJacobianStatic().transpose() + * _artInertia * math::AdInvT(this->getRelativeTransform(), _spatialAcc)); // Verification assert(!math::isNan(mInvMassMatrixSegment)); @@ -2338,7 +2338,7 @@ template void MultiDofJoint::addInvMassMatrixSegmentTo(Eigen::Vector6d& _acc) { // - _acc += getLocalJacobianStatic() * mInvMassMatrixSegment; + _acc += getRelativeJacobianStatic() * mInvMassMatrixSegment; } //============================================================================== @@ -2346,7 +2346,7 @@ template Eigen::VectorXd MultiDofJoint::getSpatialToGeneralized( const Eigen::Vector6d& _spatial) { - return getLocalJacobianStatic().transpose() * _spatial; + return getRelativeJacobianStatic().transpose() * _spatial; } } // namespace dynamics diff --git a/dart/simulation/World.cpp b/dart/simulation/World.cpp index 77857f3ffa288..e66827dc63677 100644 --- a/dart/simulation/World.cpp +++ b/dart/simulation/World.cpp @@ -496,13 +496,19 @@ std::set World::removeAllSimpleFrames() bool World::checkCollision(bool checkAllCollisions) { collision::CollisionOption option; + if (checkAllCollisions) - option.enableContact = true; + option.maxNumContacts = 1e+3; else - option.enableContact = false; + option.maxNumContacts = 1u; - collision::CollisionResult result; + return checkCollision(option); +} +//============================================================================== +bool World::checkCollision(const collision::CollisionOption& option, + collision::CollisionResult* result) +{ return mConstraintSolver->getCollisionGroup()->collide(option, result); } diff --git a/dart/simulation/World.hpp b/dart/simulation/World.hpp index ae4c2f6cea312..a90d594977e4d 100644 --- a/dart/simulation/World.hpp +++ b/dart/simulation/World.hpp @@ -52,9 +52,10 @@ #include "dart/common/Timer.hpp" #include "dart/common/NameManager.hpp" #include "dart/common/Subject.hpp" -#include "dart/simulation/Recording.hpp" #include "dart/dynamics/SimpleFrame.hpp" #include "dart/dynamics/Skeleton.hpp" +#include "dart/collision/Option.hpp" +#include "dart/simulation/Recording.hpp" namespace dart { @@ -172,12 +173,27 @@ class World : public virtual common::Subject std::set removeAllSimpleFrames(); //-------------------------------------------------------------------------- - // Kinematics + // Collision checking //-------------------------------------------------------------------------- - /// Return whether there is any collision between bodies - bool checkCollision(bool _checkAllCollisions = false); - + /// Deprecated. Please use checkCollision(~) instead. + DEPRECATED(6.0) + bool checkCollision(bool checkAllCollisions); + + /// Perform collision checking with 'option' over all the feasible collision + /// pairs in this World, and the result will be stored 'result'. If no + /// argument is passed in then it will return just whether there is collision + /// or not without the contact information such as contact point, normal, and + /// penetration depth. + bool checkCollision( + const collision::CollisionOption& option + = collision::CollisionOption(false, 1u, nullptr), + collision::CollisionResult* result = nullptr); + + /// Return the collision checking result of the last simulation step. If this + /// world hasn't stepped forward yet, then the result would be empty. Note + /// that this function does not return the collision checking result of + /// World::checkCollision(). const collision::CollisionResult& getLastCollisionResult() const; //-------------------------------------------------------------------------- diff --git a/debian/changelog b/debian/changelog index b338b13e9d352..b4aa5ed16d11a 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,8 +1,81 @@ dart6 (6.0.0) unstable; urgency=medium + * Added 'Node', 'Aspect', 'State', and 'Properties' + * Added mathematical constants and user-defined literals for radian, degree, and pi + * Added 'ShapeFrame' and 'ShapeNode' + * Added 'BoundingBox' + * Added convenient functions for setting joint limits + * Added more description on 'InverseKinematics::solve()' + * Added API for utilizing analytical inverse kinematics + * Added color property to 'Marker' + * Improved 'Skeleton' to clone 'State' as well + * Improved 'ReferentialSkeleton' to be able to add and remove 'BodyNode's and 'DegreeOfFreedom's to/from 'Group's freely + * Changed 'Marker' into 'Node' + * Renamed 'Joint::get/setLocal[~]' to 'Joint::get/setRelative[~]' + * Renamed 'PositionLimited' to 'PositionLimitEnforced' + * Fixed initialization of joint position and velocity + * Fixed 'InverseKinematics' when it's used with 'FreeJoint' and 'BallJoint' + * Fixed ambiguous overload on 'MetaSkeleton::getLinearJacobianDeriv' + * Added 'get/setLCPSolver' functions to 'ConstraintSolver' + * Added 'ServoMotorConstraint' as a preliminary implementation for 'SERVO' actuator type + * Improved 'ConstraintSolver' to obey C++11 ownership conventions + * Fixed segfualting of 'DantzigLCPSolver' when the constraint dimension is zero + * Fixed missing implementations in ConstrainedGroup + * Fixed incorrect applying of joint constraint impulses + * Deprecated 'draw()' functions of dynamics classes + * Added 'CollisionGroup' and refactored 'CollisionDetector' to be more versatile + * Improved API for self collision checking options + * Deprecated 'BodyNode::isColliding'; collision sets are moved to 'CollisionResult' + * Added back VSK parser + * Fixed segfault of 'SdfParser' when 'nullptr' 'ResourceRetriever' is passed + * Merged 'renderer' namespace into 'gui' namespace + * Moved 'osgDart' under 'dart::gui' namespace as 'dart::gui::osg' + * Fixed GlutWindow::screenshot() + * Fixed 'World::clone()' didn't clone the collision detector + * Fixed bug of 'World' concurrency + * Added 'make_unique' that was omitted from C++11 + * Added missing 'override' keywords + * Added gcc warning flag '-Wextra' + * Improved memory management of 'constraint' namespace + * Changed the extension of headers from '.h' to '.hpp' + * Changed Doxyfile to gnerate tag file + * Changed the convention to use 'std::size_t' over 'size_t' + * Changed CMake to configure preprocessors using '#cmakedefine' + * Updated copyright years + * Renamed directory name 'apps' to 'examples' + * Fixed warnings of unused variables in release mode + * Fixed typo of 'getNumPluralAddoName' in utility macro + * Fixed linker error by adding namespace-scope definitions for 'constexpr static' members + * Fixed segfault from nullptr meshes + * Fixed typo of tutorial with minor improvements + * Fixed 'NameManager::removeEntries(~)' called a function that does not exist + * Fixed missing definitions for various functions + * Fixed const correctness of 'BodyNode::getMomentsOfInertia()' + * Fixed 'ftel' bug in Linux with an workaround + * Removed unnecessary 'virtual' keyword for overriding functions + * Removed deprecated APIs in DART 5 + * Added CMake target for code coverage testing, and automatic reporting * Added missing 'liburdfdom-dev' dependency in Ubuntu package - - -- Jeongseok Lee Sun, 17 Apr 2016 00:00:00 -0500 + * Modulized DART libraries + * Improved Travis-CI script + * Improved CMake script by splitting tutorials, examples, and tests into separate targets + * Improved wording of the cmake warning messages for ASSIMP + * Changed Travis-CI to treat warning as errors using '-Werror' flags + * Changed Travis-CI to test DART with bullet collision detector + * Changed the minimum requirement of Visual Studio version to 2015 + * Changed CMake to build gui::osg examples when 'DART_BUILD_EXAMPLES' is on + * Simplfied Travis-CI tests for general pushes + * Fixed Eigen memory alignment issue in testCollision.cpp + * Fixed 'BULLET_INCLUDE_DIRS' in 'DARTConfig.cmake' + * Fixed linking with Bullet on OS X El Capitan by supporting for Bullet built with double precision + * Fixed FCL version check logic in the main 'CMakeLists.txt' + * Fixed 'find_package(DART)' on optimizer components + * Fixed linking against '${DART_LIBRARIES}' not working in Ubuntu 14.04 + * Fixed Visual Studio 2015 build errors + * Removed OpenGL dependency from 'dart' library + * Removed version check for Bullet + + -- Jeongseok Lee Tue, 10 May 2016 12:00:00 -0500 dart (5.1.1) unstable; urgency=medium diff --git a/debian/libdart6-dev.install b/debian/libdart6-dev.install index e4b1376bc30ed..5732466702766 100644 --- a/debian/libdart6-dev.install +++ b/debian/libdart6-dev.install @@ -1,5 +1,5 @@ -usr/include/dart/dart.h -usr/include/dart/config.h +usr/include/dart/dart.hpp +usr/include/dart/config.hpp usr/include/dart/collision/* usr/include/dart/common/* usr/include/dart/constraint/* diff --git a/tutorials/tutorialBiped-Finished.cpp b/tutorials/tutorialBiped-Finished.cpp index c1c7d38e8bd9c..abe2fab0cceb1 100644 --- a/tutorials/tutorialBiped-Finished.cpp +++ b/tutorials/tutorialBiped-Finished.cpp @@ -311,7 +311,8 @@ SkeletonPtr loadBiped() biped->getJoint(i)->setPositionLimitEnforced(true); // Enable self collision check but ignore adjacent bodies - biped->enableSelfCollision(); + biped->enableSelfCollisionCheck(); + biped->disableAdjacentBodyCheck(); return biped; } diff --git a/tutorials/tutorialCollisions-Finished.cpp b/tutorials/tutorialCollisions-Finished.cpp index 044e8736c42e4..f5eba4487c450 100644 --- a/tutorials/tutorialCollisions-Finished.cpp +++ b/tutorials/tutorialCollisions-Finished.cpp @@ -221,7 +221,7 @@ class MyWindow : public dart::gui::SimWindow dart::collision::CollisionOption option; dart::collision::CollisionResult result; - bool collision = collisionGroup->collide(newGroup.get(), option, result); + bool collision = collisionGroup->collide(newGroup.get(), option, &result); // If the new object is not in collision if(!collision) diff --git a/tutorials/tutorialDominoes-Finished.cpp b/tutorials/tutorialDominoes-Finished.cpp index 3e08077bbaeac..10e60b02890ad 100644 --- a/tutorials/tutorialDominoes-Finished.cpp +++ b/tutorials/tutorialDominoes-Finished.cpp @@ -272,7 +272,7 @@ class MyWindow : public dart::gui::SimWindow dart::collision::CollisionOption option; dart::collision::CollisionResult result; bool dominoCollision - = collisionGroup->collide(newGroup.get(), option, result); + = collisionGroup->collide(newGroup.get(), option, &result); // If the new domino is not penetrating an existing one if(!dominoCollision) diff --git a/unittests/TestHelpers.hpp b/unittests/TestHelpers.hpp index 9edd062e7f137..caff3b70c6f0b 100644 --- a/unittests/TestHelpers.hpp +++ b/unittests/TestHelpers.hpp @@ -262,7 +262,7 @@ SkeletonPtr createNLinkRobot(int _n, Vector3d dim, TypeOfDOF type, assert(_n > 0); SkeletonPtr robot = Skeleton::create(); - robot->disableSelfCollision(); + robot->disableSelfCollisionCheck(); // Create the first link, the joint with the ground and its shape BodyNode::Properties node(BodyNode::AspectProperties("link1")); @@ -329,7 +329,7 @@ SkeletonPtr createNLinkPendulum(size_t numBodyNodes, assert(numBodyNodes > 0); SkeletonPtr robot = Skeleton::create(); - robot->disableSelfCollision(); + robot->disableSelfCollisionCheck(); // Create the first link, the joint with the ground and its shape BodyNode::Properties node(BodyNode::AspectProperties("link1")); diff --git a/unittests/testAspect.cpp b/unittests/testAspect.cpp index 970847e48d62a..989c8fc259691 100644 --- a/unittests/testAspect.cpp +++ b/unittests/testAspect.cpp @@ -228,8 +228,8 @@ class StateAspectTest : public dart::common::AspectWithState< { public: - StateAspectTest(Composite* mgr, const StateData& state = StateData()) - : dart::common::AspectWithState(mgr, state) + StateAspectTest(const StateData& state = StateData()) + : dart::common::AspectWithState(state) { } @@ -246,15 +246,15 @@ class GenericAspect : public Aspect, public Subject { public: - GenericAspect(Composite* composite) - : Aspect(composite) + GenericAspect() + : Aspect() { // Do nothing } - std::unique_ptr cloneAspect(Composite* newComposite) const override final + std::unique_ptr cloneAspect() const override final { - return dart::common::make_unique(newComposite); + return dart::common::make_unique(); } }; @@ -263,15 +263,15 @@ class SpecializedAspect : public Aspect, public Subject { public: - SpecializedAspect(Composite* composite) - : Aspect(composite) + SpecializedAspect() + : Aspect() { // Do nothing } - std::unique_ptr cloneAspect(Composite* newComposite) const override final + std::unique_ptr cloneAspect() const override final { - return dart::common::make_unique(newComposite); + return dart::common::make_unique(); } }; @@ -294,35 +294,35 @@ class StatefulAspect : public Aspect, public Subject using State = Aspect::MakeState; using Properties = Aspect::MakeProperties; - StatefulAspect(Composite* mgr) - : Aspect(mgr) + StatefulAspect() + : Aspect() { // Do nothing } - StatefulAspect(Composite* mgr, const StatefulAspect& other) - : Aspect(mgr), + StatefulAspect(const StatefulAspect& other) + : Aspect(), mState(other.mState), mProperties(other.mProperties) { // Do nothing } - StatefulAspect(Composite* mgr, const T& state) - : Aspect(mgr), mState(state) + StatefulAspect(const T& state) + : Aspect(), mState(state) { // Do nothing } - StatefulAspect(Composite* mgr, const T& state, const T& properties) - : Aspect(mgr), + StatefulAspect(const T& state, const T& properties) + : Aspect(), mState(state), mProperties(properties) { // Do nothing } - std::unique_ptr cloneAspect(Composite* newComposite) const override final + std::unique_ptr cloneAspect() const override final { - return dart::common::make_unique(newComposite, *this); + return dart::common::make_unique(*this); } void setAspectState(const Aspect::State& otherState) override @@ -702,6 +702,20 @@ TEST(Aspect, StateAndProperties) // The constructor arguments should match the type order Composite::MakeState( doubleState, intState, charState, floatState); + + // ---- Test copying and merging ---- + Composite::Properties c_properties_1(properties); + EXPECT_FALSE(c_properties_1.has()); + + Composite::Properties c_properties_2; + c_properties_2.create(); + EXPECT_TRUE(c_properties_2.has()); + + c_properties_2.merge(c_properties_1); + EXPECT_TRUE(c_properties_2.has()); + + c_properties_2.copy(c_properties_1); + EXPECT_FALSE(c_properties_2.has()); } TEST(Aspect, Construction) diff --git a/unittests/testCollision.cpp b/unittests/testCollision.cpp index 8a75b9dafad8e..206b1bda850ac 100644 --- a/unittests/testCollision.cpp +++ b/unittests/testCollision.cpp @@ -44,13 +44,8 @@ #include "dart/config.hpp" #include "dart/common/common.hpp" #include "dart/math/math.hpp" -#include "dart/collision/CollisionGroup.hpp" -#include "dart/collision/fcl/FCLCollisionDetector.hpp" -#include "dart/collision/dart/DARTCollisionDetector.hpp" -#if HAVE_BULLET_COLLISION - #include "dart/collision/bullet/BulletCollisionDetector.hpp" -#endif #include "dart/dynamics/dynamics.hpp" +#include "dart/collision/collision.hpp" #include "dart/simulation/simulation.hpp" #include "dart/utils/utils.hpp" #include "TestHelpers.hpp" @@ -514,9 +509,9 @@ TEST_F(COLLISION, FCL_BOX_BOX) //============================================================================== void testSimpleFrames(const std::shared_ptr& cd) { - auto simpleFrame1 = std::make_shared(Frame::World()); - auto simpleFrame2 = std::make_shared(Frame::World()); - auto simpleFrame3 = std::make_shared(Frame::World()); + auto simpleFrame1 = Eigen::make_aligned_shared(Frame::World()); + auto simpleFrame2 = Eigen::make_aligned_shared(Frame::World()); + auto simpleFrame3 = Eigen::make_aligned_shared(Frame::World()); ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); ShapePtr shape2(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); @@ -541,24 +536,56 @@ void testSimpleFrames(const std::shared_ptr& cd) + group2->getNumShapeFrames() + group3->getNumShapeFrames()); + for(std::size_t i=0; i < group1->getNumShapeFrames(); ++i) + EXPECT_EQ(groupAll->getShapeFrame(i), group1->getShapeFrame(i)); + + std::size_t start = group1->getNumShapeFrames(); + std::size_t end = start + group2->getNumShapeFrames(); + for(std::size_t i=start; i < end; ++i) + EXPECT_EQ(groupAll->getShapeFrame(i), group2->getShapeFrame(i-start)); + + start = start + group2->getNumShapeFrames(); + end = start + group3->getNumShapeFrames(); + for(std::size_t i=start; i < end; ++i) + EXPECT_EQ(groupAll->getShapeFrame(i), group3->getShapeFrame(i-start)); + collision::CollisionOption option; collision::CollisionResult result; simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); simpleFrame2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); simpleFrame3->setTranslation(Eigen::Vector3d(2.2, 0.0, 0.0)); - EXPECT_FALSE(group1->collide(option, result)); - EXPECT_FALSE(group2->collide(option, result)); - EXPECT_FALSE(group3->collide(option, result)); - EXPECT_FALSE(groupAll->collide(option, result)); + EXPECT_FALSE(group1->collide(option, &result)); + EXPECT_FALSE(group2->collide(option, &result)); + EXPECT_FALSE(group3->collide(option, &result)); + EXPECT_FALSE(groupAll->collide(option, &result)); simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); simpleFrame2->setTranslation(Eigen::Vector3d(0.5, 0.0, 0.0)); simpleFrame3->setTranslation(Eigen::Vector3d(1.0, 0.0, 0.0)); - EXPECT_TRUE(group1->collide(group2.get(), option, result)); - EXPECT_TRUE(group1->collide(group2.get(), option, result)); - EXPECT_TRUE(group2->collide(group3.get(), option, result)); - EXPECT_TRUE(groupAll->collide(option, result)); + EXPECT_TRUE(group1->collide(group2.get(), option, &result)); + EXPECT_TRUE(group1->collide(group2.get(), option, &result)); + EXPECT_TRUE(group2->collide(group3.get(), option, &result)); + EXPECT_TRUE(groupAll->collide(option, &result)); + + auto group23 = cd->createCollisionGroup( + simpleFrame2.get(), simpleFrame3.get()); + simpleFrame1->setTranslation(Eigen::Vector3d::Zero()); + simpleFrame2->setTranslation(Eigen::Vector3d(1.1, 0.0, 0.0)); + simpleFrame3->setTranslation(Eigen::Vector3d(1.6, 0.0, 0.0)); + EXPECT_FALSE(group1->collide(group2.get())); + EXPECT_FALSE(group1->collide(group3.get())); + EXPECT_TRUE(group2->collide(group3.get())); + EXPECT_TRUE(group23->collide()); + if (cd->getType() == BulletCollisionDetector::getStaticType()) + { + dtwarn << "Skipping group-group test for 'bullet' collision detector. " + << "Please see Issue #717 for the detail.\n"; + } + else + { + EXPECT_FALSE(group1->collide(group23.get())); + } } //============================================================================== @@ -610,8 +637,8 @@ bool checkBoundingBox(const Eigen::Vector3d& min, const Eigen::Vector3d& max, void testBoxBox(const std::shared_ptr& cd, double tol = 1e-12) { - auto simpleFrame1 = std::make_shared(Frame::World()); - auto simpleFrame2 = std::make_shared(Frame::World()); + auto simpleFrame1 = Eigen::make_aligned_shared(Frame::World()); + auto simpleFrame2 = Eigen::make_aligned_shared(Frame::World()); ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); ShapePtr shape2(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5))); @@ -636,7 +663,7 @@ void testBoxBox(const std::shared_ptr& cd, collision::CollisionOption option; collision::CollisionResult result; - EXPECT_TRUE(group1->collide(group2.get(), option, result)); + EXPECT_TRUE(group1->collide(group2.get(), option, &result)); Eigen::Vector3d min = Eigen::Vector3d(-0.25, 0.25, 0.0); Eigen::Vector3d max = Eigen::Vector3d(0.25, 0.5, 0.0); @@ -695,9 +722,9 @@ TEST_F(COLLISION, BoxBox) //============================================================================== void testOptions(const std::shared_ptr& cd) { - auto simpleFrame1 = std::make_shared(Frame::World()); - auto simpleFrame2 = std::make_shared(Frame::World()); - auto simpleFrame3 = std::make_shared(Frame::World()); + auto simpleFrame1 = Eigen::make_aligned_shared(Frame::World()); + auto simpleFrame2 = Eigen::make_aligned_shared(Frame::World()); + auto simpleFrame3 = Eigen::make_aligned_shared(Frame::World()); ShapePtr shape1(new BoxShape(Eigen::Vector3d(1.0, 1.0, 1.0))); ShapePtr shape2(new BoxShape(Eigen::Vector3d(0.5, 0.5, 0.5))); @@ -721,22 +748,35 @@ void testOptions(const std::shared_ptr& cd) result.clear(); option.maxNumContacts = 1000u; - option.binaryCheck = false; - EXPECT_TRUE(group->collide(option, result)); + EXPECT_TRUE(group->collide(option, &result)); EXPECT_EQ(result.getNumContacts(), 4u); result.clear(); option.maxNumContacts = 2u; - option.binaryCheck = false; - EXPECT_TRUE(group->collide(option, result)); + EXPECT_TRUE(group->collide(option, &result)); EXPECT_EQ(result.getNumContacts(), 2u); group->addShapeFrame(simpleFrame3.get()); result.clear(); - option.maxNumContacts = 1e+3; - option.binaryCheck = true; - EXPECT_TRUE(group->collide(option, result)); + option.maxNumContacts = 1u; + EXPECT_TRUE(group->collide(option, &result)); EXPECT_EQ(result.getNumContacts(), 1u); + + // Binary check without passing result + EXPECT_TRUE(group->collide(option)); + + // Binary check without passing option and result + EXPECT_TRUE(group->collide()); + + // Zero maximum number of contacts + option.maxNumContacts = 0u; + option.enableContact = true; + EXPECT_TRUE(group->collide()); + EXPECT_FALSE(group->collide(option)); + EXPECT_FALSE(group->collide(option, nullptr)); + EXPECT_FALSE(group->collide(option, &result)); + EXPECT_EQ(result.getNumContacts(), 0u); + EXPECT_FALSE(result.isCollision()); } //============================================================================== @@ -771,6 +811,89 @@ TEST_F(COLLISION, Options) testOptions(dart); } +//============================================================================== +void testFilter(const std::shared_ptr& cd) +{ + // Create two bodies skeleton. The two bodies are placed at the same position + // with the same size shape so that they collide by default. + auto skel = Skeleton::create(); + auto shape = std::make_shared(Eigen::Vector3d(1, 1, 1)); + auto pair0 = skel->createJointAndBodyNodePair(nullptr); + auto* body0 = pair0.second; + body0->createShapeNodeWith(shape); + auto pair1 = body0->createChildJointAndBodyNodePair(); + auto* body1 = pair1.second; + body1->createShapeNodeWith(shape); + + // Create a collision group from the skeleton + auto group = cd->createCollisionGroup(skel.get()); + EXPECT_EQ(group->getNumShapeFrames(), 2u); + + // Default collision filter for Skeleton + CollisionOption option; + option.collisionFilter = std::make_shared(); + + skel->enableSelfCollisionCheck(); + skel->enableAdjacentBodyCheck(); + EXPECT_TRUE(skel->isEnabledSelfCollisionCheck()); + EXPECT_TRUE(skel->isEnabledAdjacentBodyCheck()); + EXPECT_TRUE(group->collide()); // without filter, always collision + EXPECT_TRUE(group->collide(option)); + + skel->enableSelfCollisionCheck(); + skel->disableAdjacentBodyCheck(); + EXPECT_TRUE(skel->isEnabledSelfCollisionCheck()); + EXPECT_FALSE(skel->isEnabledAdjacentBodyCheck()); + EXPECT_TRUE(group->collide()); + EXPECT_FALSE(group->collide(option)); + + skel->disableSelfCollisionCheck(); + skel->enableAdjacentBodyCheck(); + EXPECT_FALSE(skel->isEnabledSelfCollisionCheck()); + EXPECT_TRUE(skel->isEnabledAdjacentBodyCheck()); + EXPECT_TRUE(group->collide()); + EXPECT_FALSE(group->collide(option)); + + skel->disableSelfCollisionCheck(); + skel->disableAdjacentBodyCheck(); + EXPECT_FALSE(skel->isEnabledSelfCollisionCheck()); + EXPECT_FALSE(skel->isEnabledAdjacentBodyCheck()); + EXPECT_TRUE(group->collide()); + EXPECT_FALSE(group->collide(option)); +} + +//============================================================================== +TEST_F(COLLISION, Filter) +{ + auto fcl_mesh_dart = FCLCollisionDetector::create(); + fcl_mesh_dart->setPrimitiveShapeType(FCLCollisionDetector::MESH); + fcl_mesh_dart->setContactPointComputationMethod(FCLCollisionDetector::DART); + testFilter(fcl_mesh_dart); + + // auto fcl_prim_fcl = FCLCollisionDetector::create(); + // fcl_prim_fcl->setPrimitiveShapeType(FCLCollisionDetector::MESH); + // fcl_prim_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testFilter(fcl_prim_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::DART); + // testFilter(fcl_mesh_fcl); + + // auto fcl_mesh_fcl = FCLCollisionDetector::create(); + // fcl_mesh_fcl->setPrimitiveShapeType(FCLCollisionDetector::PRIMITIVE); + // fcl_mesh_fcl->setContactPointComputationMethod(FCLCollisionDetector::FCL); + // testFilter(fcl_mesh_fcl); + +#if HAVE_BULLET_COLLISION + auto bullet = BulletCollisionDetector::create(); + testFilter(bullet); +#endif + + auto dart = DARTCollisionDetector::create(); + testFilter(dart); +} + //============================================================================== void testCreateCollisionGroups(const std::shared_ptr& cd) { @@ -799,9 +922,22 @@ void testCreateCollisionGroups(const std::shared_ptr& cd) auto shapeNodeGroup1 = cd->createCollisionGroup(boxShapeNode1); auto shapeNodeGroup2 = cd->createCollisionGroup(boxShapeNode2); - EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get(), option, result)); - EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get(), option, result)); - EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get(), option, result)); + EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get(), option, &result)); + EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get(), option, &result)); + EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get(), option, &result)); + + // Binary check without passing option + auto oldMaxNumContacts = option.maxNumContacts; + option.maxNumContacts = 1u; + EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get(), option)); + EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get(), option)); + EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get(), option)); + option.maxNumContacts = oldMaxNumContacts; + + // Binary check without passing option and result + EXPECT_TRUE(skeletonGroup1->collide(skeletonGroup2.get())); + EXPECT_TRUE(bodyNodeGroup1->collide(bodyNodeGroup2.get())); + EXPECT_TRUE(shapeNodeGroup1->collide(shapeNodeGroup2.get())); // Regression test for #666 auto world = common::make_unique(); diff --git a/unittests/testDynamics.cpp b/unittests/testDynamics.cpp index dbb65846c2021..3b46437124bd4 100644 --- a/unittests/testDynamics.cpp +++ b/unittests/testDynamics.cpp @@ -1093,8 +1093,8 @@ void testForwardKinematicsSkeleton(const dynamics::SkeletonPtr& skel) auto body = skel->getBodyNode(k); auto joint = skel->getJoint(k); auto parentBody = body->getParentBodyNode(); - Eigen::MatrixXd S = joint->getLocalJacobian(); - Eigen::MatrixXd dS = joint->getLocalJacobianTimeDeriv(); + Eigen::MatrixXd S = joint->getRelativeJacobian(); + Eigen::MatrixXd dS = joint->getRelativeJacobianTimeDeriv(); Eigen::VectorXd jointQ = joint->getPositions(); Eigen::VectorXd jointDQ = joint->getVelocities(); Eigen::VectorXd jointDDQ = joint->getAccelerations(); diff --git a/unittests/testJoints.cpp b/unittests/testJoints.cpp index ecd3cdf9ff2e7..fe2e45be23a57 100644 --- a/unittests/testJoints.cpp +++ b/unittests/testJoints.cpp @@ -172,9 +172,9 @@ void JOINTS::kinematicsTest(const typename JointType::Properties& _properties) if (dof == 0) return; - Eigen::Isometry3d T = joint->getLocalTransform(); - Jacobian J = joint->getLocalJacobian(); - Jacobian dJ = joint->getLocalJacobianTimeDeriv(); + Eigen::Isometry3d T = joint->getRelativeTransform(); + Jacobian J = joint->getRelativeJacobian(); + Jacobian dJ = joint->getRelativeJacobianTimeDeriv(); //-------------------------------------------------------------------------- // Test T @@ -191,13 +191,13 @@ void JOINTS::kinematicsTest(const typename JointType::Properties& _properties) // a Eigen::VectorXd q_a = q; joint->setPositions(q_a); - Eigen::Isometry3d T_a = joint->getLocalTransform(); + Eigen::Isometry3d T_a = joint->getRelativeTransform(); // b Eigen::VectorXd q_b = q; q_b(i) += q_delta; joint->setPositions(q_b); - Eigen::Isometry3d T_b = joint->getLocalTransform(); + Eigen::Isometry3d T_b = joint->getRelativeTransform(); // Eigen::Isometry3d Tinv_a = T_a.inverse(); @@ -237,13 +237,13 @@ void JOINTS::kinematicsTest(const typename JointType::Properties& _properties) // a Eigen::VectorXd q_a = q; joint->setPositions(q_a); - Jacobian J_a = joint->getLocalJacobian(); + Jacobian J_a = joint->getRelativeJacobian(); // b Eigen::VectorXd q_b = q; q_b(i) += q_delta; joint->setPositions(q_b); - Jacobian J_b = joint->getLocalJacobian(); + Jacobian J_b = joint->getRelativeJacobian(); // Jacobian dJ_dq = (J_b - J_a) / q_delta; @@ -273,7 +273,7 @@ void JOINTS::kinematicsTest(const typename JointType::Properties& _properties) if (joint->getNumDofs() == 0) return; - Eigen::Isometry3d T = joint->getLocalTransform(); + Eigen::Isometry3d T = joint->getRelativeTransform(); EXPECT_TRUE(math::verifyTransform(T)); } } @@ -516,7 +516,7 @@ void testJointCoulombFrictionForce(double _timeStep) dynamics::SkeletonPtr pendulum = myWorld->getSkeleton("double_pendulum"); EXPECT_TRUE(pendulum != nullptr); - pendulum->disableSelfCollision(); + pendulum->disableSelfCollisionCheck(); dynamics::Joint* joint0 = pendulum->getJoint("joint0"); dynamics::Joint* joint1 = pendulum->getJoint("joint1"); @@ -635,7 +635,7 @@ SkeletonPtr createPendulum(Joint::ActuatorType actType) SkeletonPtr pendulum = createNLinkPendulum(1, dim, DOF_ROLL, offset); EXPECT_NE(pendulum, nullptr); - pendulum->disableSelfCollision(); + pendulum->disableSelfCollisionCheck(); for (std::size_t i = 0; i < pendulum->getNumBodyNodes(); ++i) { @@ -829,7 +829,7 @@ TEST_F(JOINTS, JOINT_COULOMB_FRICTION_AND_POSITION_LIMIT) dynamics::SkeletonPtr pendulum = myWorld->getSkeleton("double_pendulum"); EXPECT_TRUE(pendulum != nullptr); - pendulum->disableSelfCollision(); + pendulum->disableSelfCollisionCheck(); dynamics::Joint* joint0 = pendulum->getJoint("joint0"); dynamics::Joint* joint1 = pendulum->getJoint("joint1"); diff --git a/unittests/testSkeleton.cpp b/unittests/testSkeleton.cpp index e9f25adfb559e..6ad0c44bad19e 100644 --- a/unittests/testSkeleton.cpp +++ b/unittests/testSkeleton.cpp @@ -1228,11 +1228,11 @@ TEST(Skeleton, Updating) Joint* joint0 = skeleton->getJoint(0); Joint* joint1 = skeleton->getJoint(1); - math::Jacobian J0i = joint0->getLocalJacobian(); + math::Jacobian J0i = joint0->getRelativeJacobian(); joint0->get()->setProperties( joint1->get()->getProperties()); - math::Jacobian J0f = joint0->getLocalJacobian(); + math::Jacobian J0f = joint0->getRelativeJacobian(); EXPECT_FALSE(equals(J0i, J0f)); // PrismaticJoint @@ -1241,10 +1241,10 @@ TEST(Skeleton, Updating) joint0 = skeleton->getJoint(0); joint1 = skeleton->getJoint(1); - J0i = joint0->getLocalJacobian(); + J0i = joint0->getRelativeJacobian(); joint0->get()->setProperties( joint1->get()->getProperties()); - J0f = joint0->getLocalJacobian(); + J0f = joint0->getRelativeJacobian(); EXPECT_FALSE(equals(J0i, J0f)); skeleton = Skeleton::create(); @@ -1253,14 +1253,14 @@ TEST(Skeleton, Updating) screw->setAxis(Eigen::Vector3d::UnitX()); screw->setPitch(2); - J0i = screw->getLocalJacobian(); + J0i = screw->getRelativeJacobian(); screw->setAxis(Eigen::Vector3d::UnitY()); - J0f = screw->getLocalJacobian(); + J0f = screw->getRelativeJacobian(); EXPECT_FALSE(equals(J0i, J0f)); J0i = J0f; screw->setPitch(3); - J0f = screw->getLocalJacobian(); + J0f = screw->getRelativeJacobian(); EXPECT_FALSE(equals(J0i, J0f)); }