diff --git a/dart/collision/CollisionFilter.cpp b/dart/collision/CollisionFilter.cpp
index 1732727c4df6b..0e72412d2d56a 100644
--- a/dart/collision/CollisionFilter.cpp
+++ b/dart/collision/CollisionFilter.cpp
@@ -38,44 +38,114 @@ namespace dart {
 namespace collision {
 
 //==============================================================================
-bool BodyNodeCollisionFilter::needCollision(
+bool CollisionFilter::needCollision(
+    const CollisionObject* object1, const CollisionObject* object2) const
+{
+  return !ignoresCollision(object1, object2);
+}
+
+//==============================================================================
+void CompositeCollisionFilter::addCollisionFilter(const CollisionFilter* filter)
+{
+  // nullptr is not an allowed filter
+  if (!filter)
+    return;
+
+  mFilters.insert(filter);
+}
+
+//==============================================================================
+void CompositeCollisionFilter::removeCollisionFilter(
+const CollisionFilter* filter)
+{
+  mFilters.erase(filter);
+}
+
+//==============================================================================
+void CompositeCollisionFilter::removeAllCollisionFilters()
+{
+  mFilters.clear();
+}
+
+//==============================================================================
+bool CompositeCollisionFilter::ignoresCollision(
+    const CollisionObject* object1, const CollisionObject* object2) const
+{
+  for (const auto* filter : mFilters)
+  {
+    if (filter->ignoresCollision(object1, object2))
+      return true;
+  }
+
+  return false;
+}
+
+//==============================================================================
+void BodyNodeCollisionFilter::addBodyNodePairToBlackList(
+    const dynamics::BodyNode* bodyNode1, const dynamics::BodyNode* bodyNode2)
+{
+  mBodyNodeBlackList.addPair(bodyNode1, bodyNode2);
+}
+
+//==============================================================================
+void BodyNodeCollisionFilter::removeBodyNodePairFromBlackList(
+    const dynamics::BodyNode* bodyNode1, const dynamics::BodyNode* bodyNode2)
+{
+  mBodyNodeBlackList.removePair(bodyNode1, bodyNode2);
+}
+
+//==============================================================================
+void BodyNodeCollisionFilter::removeAllBodyNodePairsFromBlackList()
+{
+  mBodyNodeBlackList.removeAllPairs();
+}
+
+//==============================================================================
+bool BodyNodeCollisionFilter::ignoresCollision(
     const collision::CollisionObject* object1,
     const collision::CollisionObject* object2) const
 {
   if (object1 == object2)
-    return false;
+    return true;
 
   auto shapeNode1 = object1->getShapeFrame()->asShapeNode();
   auto shapeNode2 = object2->getShapeFrame()->asShapeNode();
 
+  // We don't filter out for non-ShapeNode because this class shouldn't have the
+  // authority to make decisions about filtering any ShapeFrames that aren't
+  // attached to a BodyNode. So here we just return false. In order to decide
+  // whether the non-ShapeNode should be ignored, please use other collision
+  // filters.
   if (!shapeNode1 || !shapeNode2)
-    return true;
-  // We assume that non-ShapeNode is always being checked collision.
+    return false;
 
   auto bodyNode1 = shapeNode1->getBodyNodePtr();
   auto bodyNode2 = shapeNode2->getBodyNodePtr();
 
   if (bodyNode1 == bodyNode2)
-    return false;
+    return true;
 
   if (!bodyNode1->isCollidable() || !bodyNode2->isCollidable())
-    return false;
+    return true;
 
   if (bodyNode1->getSkeleton() == bodyNode2->getSkeleton())
   {
     auto skeleton = bodyNode1->getSkeleton();
 
     if (!skeleton->isEnabledSelfCollisionCheck())
-      return false;
+      return true;
 
     if (!skeleton->isEnabledAdjacentBodyCheck())
     {
       if (areAdjacentBodies(bodyNode1, bodyNode2))
-        return false;
+        return true;
     }
   }
 
-  return true;
+  if (mBodyNodeBlackList.contains(bodyNode1, bodyNode2))
+    return true;
+
+  return false;
 }
 
 //==============================================================================
@@ -93,5 +163,5 @@ bool BodyNodeCollisionFilter::areAdjacentBodies(
   return false;
 }
 
-}  // namespace collision
-}  // namespace dart
+} // namespace collision
+} // namespace dart
diff --git a/dart/collision/CollisionFilter.hpp b/dart/collision/CollisionFilter.hpp
index a507d23e13df2..b578473b12acc 100644
--- a/dart/collision/CollisionFilter.hpp
+++ b/dart/collision/CollisionFilter.hpp
@@ -32,6 +32,9 @@
 #ifndef DART_COLLISION_COLLISIONFILTER_HPP_
 #define DART_COLLISION_COLLISIONFILTER_HPP_
 
+#include "dart/common/Deprecated.hpp"
+#include "dart/collision/detail/UnorderedPairs.hpp"
+
 namespace dart {
 
 namespace dynamics {
@@ -42,22 +45,78 @@ namespace collision {
 
 class CollisionObject;
 
-struct CollisionFilter
+class CollisionFilter
 {
-  virtual bool needCollision(const CollisionObject* object1,
-                             const CollisionObject* object2) const = 0;
+public:
+  /// Returns true if the given two CollisionObjects should be checked by the
+  /// collision detector, false otherwise.
+  /// \deprecated Deprecated in 6.3.0. Please use ignoreCollision instead. Note
+  /// that ignoreCollision returns logically opposite to what needCollision
+  /// returns.
+  DART_DEPRECATED(6.3)
+  bool needCollision(
+      const CollisionObject* object1, const CollisionObject* object2) const;
+
+  /// Returns true if the given two CollisionObjects should be checked by the
+  /// collision detector, false otherwise.
+  virtual bool ignoresCollision(
+      const CollisionObject* object1, const CollisionObject* object2) const = 0;
 };
 
-struct BodyNodeCollisionFilter : CollisionFilter
+class CompositeCollisionFilter : public CollisionFilter
 {
-  bool needCollision(const CollisionObject* object1,
-                     const CollisionObject* object2) const override;
+public:
+  /// Adds a collision filter to this CompositeCollisionFilter.
+  void addCollisionFilter(const CollisionFilter* filter);
+
+  /// Removes a collision filter from this CompositeCollisionFilter.
+  void removeCollisionFilter(const CollisionFilter* filter);
+
+  /// Removes all the collision filters from this CompositeCollisionFilter.
+  void removeAllCollisionFilters();
+
+  // Documentation inherited
+  bool ignoresCollision(
+      const CollisionObject* object1,
+      const CollisionObject* object2) const override;
+
+protected:
+  /// Collision filters
+  std::unordered_set<const CollisionFilter*> mFilters;
+};
 
+class BodyNodeCollisionFilter : public CollisionFilter
+{
+public:
+  /// Add a BodyNode pair to the blacklist.
+  void addBodyNodePairToBlackList(
+      const dynamics::BodyNode* bodyNode1,
+      const dynamics::BodyNode* bodyNode2);
+
+  /// Remove a BodyNode pair from the blacklist.
+  void removeBodyNodePairFromBlackList(
+      const dynamics::BodyNode* bodyNode1,
+      const dynamics::BodyNode* bodyNode2);
+
+  /// Remove all the BodyNode pairs from the blacklist.
+  void removeAllBodyNodePairsFromBlackList();
+
+  // Documentation inherited
+  bool ignoresCollision(
+      const CollisionObject* object1,
+      const CollisionObject* object2) const override;
+
+private:
+  /// Returns true if the two BodyNodes are adjacent BodyNodes (i.e., the two
+  /// BodyNodes are connected by a Joint).
   bool areAdjacentBodies(const dynamics::BodyNode* bodyNode1,
                          const dynamics::BodyNode* bodyNode2) const;
+
+  /// List of pairs to be ignored in the collision detection.
+  detail::UnorderedPairs<dynamics::BodyNode> mBodyNodeBlackList;
 };
 
-}  // namespace collision
-}  // namespace dart
+} // namespace collision
+} // namespace dart
 
 #endif  // DART_COLLISION_COLLISIONFILTER_HPP_
diff --git a/dart/collision/bullet/BulletCollisionDetector.cpp b/dart/collision/bullet/BulletCollisionDetector.cpp
index 0a8fb66e650eb..eb182f8149691 100644
--- a/dart/collision/bullet/BulletCollisionDetector.cpp
+++ b/dart/collision/bullet/BulletCollisionDetector.cpp
@@ -195,7 +195,7 @@ void filterOutCollisions(btCollisionWorld* world)
     const auto collObj0 = static_cast<BulletCollisionObject*>(userPtr0);
     const auto collObj1 = static_cast<BulletCollisionObject*>(userPtr1);
 
-    if (!filter->needCollision(collObj0, collObj1))
+    if (filter->ignoresCollision(collObj0, collObj1))
       manifoldsToRelease.push_back(contactManifold);
   }
 
diff --git a/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp b/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp
index dc9c66f66cf1a..c3227ca3a1482 100644
--- a/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp
+++ b/dart/collision/bullet/detail/BulletCollisionDispatcher.cpp
@@ -78,7 +78,7 @@ bool BulletCollisionDispatcher::needsCollision(
   const auto collObj1
       = static_cast<BulletCollisionObject*>(body1->getUserPointer());
 
-  if (mFilter && !mFilter->needCollision(collObj0, collObj1))
+  if (mFilter && mFilter->ignoresCollision(collObj0, collObj1))
     return false;
 
   return btCollisionDispatcher::needsCollision(body0, body1);
diff --git a/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp b/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp
index 4ed67f7d76e42..98147d9b618eb 100644
--- a/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp
+++ b/dart/collision/bullet/detail/BulletOverlapFilterCallback.cpp
@@ -75,7 +75,7 @@ bool BulletOverlapFilterCallback::needBroadphaseCollision(
     const auto collObj0 = static_cast<BulletCollisionObject*>(userPtr0);
     const auto collObj1 = static_cast<BulletCollisionObject*>(userPtr1);
 
-    return filter->needCollision(collObj0, collObj1);
+    return !filter->ignoresCollision(collObj0, collObj1);
   }
 
   return collide;
diff --git a/dart/collision/dart/DARTCollisionDetector.cpp b/dart/collision/dart/DARTCollisionDetector.cpp
index 268157a959859..4125b0985040f 100644
--- a/dart/collision/dart/DARTCollisionDetector.cpp
+++ b/dart/collision/dart/DARTCollisionDetector.cpp
@@ -145,7 +145,7 @@ bool DARTCollisionDetector::collide(
     {
       auto* collObj2 = objects[j];
 
-      if (filter && !filter->needCollision(collObj1, collObj2))
+      if (filter && filter->ignoresCollision(collObj1, collObj2))
         continue;
 
       collisionFound = checkPair(collObj1, collObj2, option, result);
@@ -207,7 +207,7 @@ bool DARTCollisionDetector::collide(
     {
       auto* collObj2 = objects2[j];
 
-      if (filter && !filter->needCollision(collObj1, collObj2))
+      if (filter && filter->ignoresCollision(collObj1, collObj2))
         continue;
 
       collisionFound = checkPair(collObj1, collObj2, option, result);
diff --git a/dart/collision/detail/UnorderedPairs.hpp b/dart/collision/detail/UnorderedPairs.hpp
new file mode 100644
index 0000000000000..7d8b5b9a76b5b
--- /dev/null
+++ b/dart/collision/detail/UnorderedPairs.hpp
@@ -0,0 +1,152 @@
+/*
+ * Copyright (c) 2017, Graphics Lab, Georgia Tech Research Corporation
+ * Copyright (c) 2017, Personal Robotics Lab, Carnegie Mellon University
+ * All rights reserved.
+ *
+ * This file is provided under the following "BSD-style" License:
+ *   Redistribution and use in source and binary forms, with or
+ *   without modification, are permitted provided that the following
+ *   conditions are met:
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
+ *   CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
+ *   INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
+ *   MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ *   DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
+ *   CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
+ *   SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
+ *   LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
+ *   USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ *   AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *   LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *   ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *   POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef DART_COLLISION_DETAIL_UNORDEREDPAIRS_HPP_
+#define DART_COLLISION_DETAIL_UNORDEREDPAIRS_HPP_
+
+#include <unordered_map>
+#include <unordered_set>
+
+namespace dart {
+namespace collision {
+namespace detail {
+
+template <class T>
+class UnorderedPairs
+{
+public:
+  /// Adds a pair to this container.
+  void addPair(const T* left, const T* right);
+
+  /// Removes a pair from this container.
+  void removePair(const T* left, const T* right);
+
+  /// Removes all the pairs from this container.
+  void removeAllPairs();
+
+  /// Returns true if this container contains the pair.
+  bool contains(const T* left, const T* right) const;
+
+private:
+  /// The actual container to store pairs.
+  ///
+  /// Each pair is stored so that the key of the std::unordered_map always has
+  /// a value less than every element in the std::unordered_set that is
+  /// associated with it.
+  std::unordered_map<const T*, std::unordered_set<const T*>> mList;
+};
+
+//==============================================================================
+template <class T>
+void UnorderedPairs<T>::addPair(const T* left, const T* right)
+{
+  if (!left || !right)
+    return;
+
+  const auto* less = left;
+  const auto* greater = right;
+
+  if (less > greater)
+    std::swap(less, greater);
+
+  // Call insert in case an entry for bodyNodeLess doesn't exist. If it doesn't
+  // exist, it will be initialized with an empty set. If it does already exist,
+  // we will just get an iterator to the existing entry.
+  const auto itLess = mList.insert(
+      std::make_pair(less, std::unordered_set<const T*>())).first;
+
+  // Insert bodyNodeGreater into the set corresponding to bodyNodeLess. If the
+  // pair already existed, this will do nothing.
+  itLess->second.insert(greater);
+}
+
+//==============================================================================
+template<class T>
+void UnorderedPairs<T>::removePair(const T* left, const T* right)
+{
+  if (!left || !right)
+    return;
+
+  const auto* bodyNodeLess = left;
+  const auto* bodyNodeGreater = right;
+
+  if (bodyNodeLess > bodyNodeGreater)
+    std::swap(bodyNodeLess, bodyNodeGreater);
+
+  // Remove the pair only when it already exists
+  const auto resultLeft = mList.find(bodyNodeLess);
+  const bool foundLeft = (resultLeft != mList.end());
+  if (foundLeft)
+  {
+    auto& associatedRights = resultLeft->second;
+    associatedRights.erase(bodyNodeGreater);
+
+    if (associatedRights.empty())
+      mList.erase(resultLeft);
+  }
+}
+
+//==============================================================================
+template<class T>
+void UnorderedPairs<T>::removeAllPairs()
+{
+  mList.clear();
+}
+
+//==============================================================================
+template<class T>
+bool UnorderedPairs<T>::contains(const T* left, const T* right) const
+{
+  const auto* less = left;
+  const auto* greater = right;
+
+  if (less > greater)
+    std::swap(less, greater);
+
+  const auto resultLeft = mList.find(less);
+  const bool foundLeft = (resultLeft != mList.end());
+  if (foundLeft)
+  {
+    auto& associatedRights = resultLeft->second;
+
+    const auto resultRight = associatedRights.find(greater);
+    const bool foundRight = (resultRight != associatedRights.end());
+    if (foundRight)
+      return true;
+  }
+
+  return false;
+}
+
+} // namespace detail
+} // namespace collision
+} // namespace dart
+
+#endif // DART_COLLISION_DETAIL_UNORDEREDPAIRS_HPP_
diff --git a/dart/collision/fcl/FCLCollisionDetector.cpp b/dart/collision/fcl/FCLCollisionDetector.cpp
index 67000e7f671b8..f522b0ee7e33a 100644
--- a/dart/collision/fcl/FCLCollisionDetector.cpp
+++ b/dart/collision/fcl/FCLCollisionDetector.cpp
@@ -1099,7 +1099,7 @@ bool collisionCallback(
     assert(collisionObject1);
     assert(collisionObject2);
 
-    if (!filter->needCollision(collisionObject2, collisionObject1))
+    if (filter->ignoresCollision(collisionObject2, collisionObject1))
       return collData->done;
   }
 
diff --git a/dart/collision/ode/OdeCollisionDetector.cpp b/dart/collision/ode/OdeCollisionDetector.cpp
index 379c8f6452ed5..ec5fda5ace4fb 100644
--- a/dart/collision/ode/OdeCollisionDetector.cpp
+++ b/dart/collision/ode/OdeCollisionDetector.cpp
@@ -267,7 +267,7 @@ void CollisionCallback(void* data, dGeomID o1, dGeomID o2)
   assert(collObj1);
   assert(collObj2);
 
-  if (filter && !filter->needCollision(collObj1, collObj2))
+  if (filter && filter->ignoresCollision(collObj1, collObj2))
       return;
 
   // Perform narrow-phase collision detection
diff --git a/dart/common/Factory.hpp b/dart/common/Factory.hpp
index 70aefeb9acc88..2ac4c51cc60c1 100644
--- a/dart/common/Factory.hpp
+++ b/dart/common/Factory.hpp
@@ -98,6 +98,9 @@ class Factory
   // (see: https://github.com/dartsim/dart/pull/845)
 
 private:
+  template <typename Derived>
+  static HeldT defaultCreator(Args&&... args);
+
   /// Object creator function map.
   CreatorMap mCreatorMap;
 };
diff --git a/dart/common/detail/Factory-impl.hpp b/dart/common/detail/Factory-impl.hpp
index d2c5434f4a5d5..06cc704db0189 100644
--- a/dart/common/detail/Factory-impl.hpp
+++ b/dart/common/detail/Factory-impl.hpp
@@ -113,11 +113,7 @@ void Factory<KeyT, BaseT, HeldT, Args...>::registerCreator(
 {
   return registerCreator(
       key,
-      [](Args&&... args) -> HeldT
-      {
-        return detail::DefaultCreator<Derived, HeldT, Args...>::run(
-            std::forward<Args>(args)...);
-      }
+      &Factory::defaultCreator<Derived>
   );
 }
 
@@ -178,6 +174,18 @@ HeldT Factory<KeyT, BaseT, HeldT, Args...>::create(
   return it->second(std::forward<Args>(args)...);
 }
 
+//==============================================================================
+template <typename KeyT,
+          typename BaseT,
+          typename HeldT,
+          typename... Args>
+template <typename Derived>
+HeldT Factory<KeyT, BaseT, HeldT, Args...>::defaultCreator(Args&&... args)
+{
+  return detail::DefaultCreator<Derived, HeldT, Args...>::run(
+        std::forward<Args>(args)...);
+}
+
 //==============================================================================
 template <typename KeyT,
           typename BaseT,
diff --git a/dart/constraint/ConstraintSolver.cpp b/dart/constraint/ConstraintSolver.cpp
index 04079739790db..76d5b5b6b776b 100644
--- a/dart/constraint/ConstraintSolver.cpp
+++ b/dart/constraint/ConstraintSolver.cpp
@@ -259,6 +259,18 @@ collision::ConstCollisionGroupPtr ConstraintSolver::getCollisionGroup() const
   return mCollisionGroup;
 }
 
+//==============================================================================
+collision::CollisionOption& ConstraintSolver::getCollisionOption()
+{
+  return mCollisionOption;
+}
+
+//==============================================================================
+const collision::CollisionOption& ConstraintSolver::getCollisionOption() const
+{
+  return mCollisionOption;
+}
+
 //==============================================================================
 collision::CollisionResult& ConstraintSolver::getLastCollisionResult()
 {
diff --git a/dart/constraint/ConstraintSolver.hpp b/dart/constraint/ConstraintSolver.hpp
index c41f2e7e49940..0ee970a35965c 100644
--- a/dart/constraint/ConstraintSolver.hpp
+++ b/dart/constraint/ConstraintSolver.hpp
@@ -50,9 +50,6 @@ class ShapeNodeCollisionObject;
 
 namespace constraint {
 
-// TODO:
-//   - RootSkeleton concept
-
 /// ConstraintSolver manages constraints and computes constraint impulses
 class ConstraintSolver
 {
@@ -125,6 +122,14 @@ class ConstraintSolver
   /// ConstraintSolver
   collision::ConstCollisionGroupPtr getCollisionGroup() const;
 
+  /// Returns collision option that is used for collision checkings in this
+  /// ConstraintSolver to generate contact constraints.
+  collision::CollisionOption& getCollisionOption();
+
+  /// Returns collision option that is used for collision checkings in this
+  /// ConstraintSolver to generate contact constraints.
+  const collision::CollisionOption& getCollisionOption() const;
+
   /// Return the last collision checking result
   collision::CollisionResult& getLastCollisionResult();
 
diff --git a/dart/constraint/WeldJointConstraint.cpp b/dart/constraint/WeldJointConstraint.cpp
index c47a39d171d27..99f17271c85f8 100644
--- a/dart/constraint/WeldJointConstraint.cpp
+++ b/dart/constraint/WeldJointConstraint.cpp
@@ -80,9 +80,22 @@ WeldJointConstraint::WeldJointConstraint(dynamics::BodyNode* _body1,
   mOldX[5] = 0.0;
 }
 
+//==============================================================================
+void WeldJointConstraint::setRelativeTransform(const Eigen::Isometry3d& _tf)
+{
+  mRelativeTransform = _tf;
+}
+
+//==============================================================================
+const Eigen::Isometry3d& WeldJointConstraint::getRelativeTransform() const
+{
+  return mRelativeTransform;
+}
+
 //==============================================================================
 WeldJointConstraint::~WeldJointConstraint()
 {
+  // Do nothing
 }
 
 //==============================================================================
diff --git a/dart/constraint/WeldJointConstraint.hpp b/dart/constraint/WeldJointConstraint.hpp
index aaebfa7c50c7e..786d3c23e61b7 100644
--- a/dart/constraint/WeldJointConstraint.hpp
+++ b/dart/constraint/WeldJointConstraint.hpp
@@ -51,6 +51,12 @@ class WeldJointConstraint : public JointConstraint
   /// Constructor that takes two bodies
   WeldJointConstraint(dynamics::BodyNode* _body1, dynamics::BodyNode* _body2);
 
+  /// Set the relative transform that this WeldJointConstraint will enforce
+  void setRelativeTransform(const Eigen::Isometry3d& _tf);
+
+  /// Get the relative transform that this WeldJointConstraint will enforce
+  const Eigen::Isometry3d& getRelativeTransform() const;
+
   /// Destructor
   virtual ~WeldJointConstraint();
 
diff --git a/dart/utils/SkelParser.cpp b/dart/utils/SkelParser.cpp
index 4b1a53c123453..5f67ddbc66744 100644
--- a/dart/utils/SkelParser.cpp
+++ b/dart/utils/SkelParser.cpp
@@ -540,8 +540,23 @@ void readVisualizationShapeNode(
   // color
   if (hasElement(vizShapeNodeEle, "color"))
   {
-    Eigen::Vector3d color = getValueVector3d(vizShapeNodeEle, "color");
-    visualAspect->setColor(color);
+    Eigen::VectorXd color = getValueVectorXd(vizShapeNodeEle, "color");
+
+    if (color.size() == 3)
+    {
+      visualAspect->setColor(static_cast<Eigen::Vector3d>(color));
+    }
+    else if (color.size() == 4)
+    {
+      visualAspect->setColor(static_cast<Eigen::Vector4d>(color));
+    }
+    else
+    {
+      dtwarn << "[readVisualizationShapeNode] Invalid format for <color> "
+             << "element; " << color.size() << "d vector is given. It should "
+             << "be either 3d vector or 4d vector (the 4th element is for "
+             << "alpha). Ignoring the given color value.\n";
+    }
   }
 }
 
diff --git a/unittests/comprehensive/test_Collision.cpp b/unittests/comprehensive/test_Collision.cpp
index acbee7afa4c17..5433b132a27d1 100644
--- a/unittests/comprehensive/test_Collision.cpp
+++ b/unittests/comprehensive/test_Collision.cpp
@@ -1158,13 +1158,20 @@ void testFilter(const std::shared_ptr<CollisionDetector>& cd)
   auto* body1 = pair1.second;
   body1->createShapeNodeWith<VisualAspect, CollisionAspect>(shape);
 
-  // Create a collision group from the skeleton
-  auto group = cd->createCollisionGroup(skel.get());
+  // Create a world and add the created skeleton
+  auto world = std::make_shared<simulation::World>();
+  auto constraintSolver = world->getConstraintSolver();
+  constraintSolver->setCollisionDetector(cd);
+  world->addSkeleton(skel);
+
+  // Get the collision group from the constraint solver
+  auto group = constraintSolver->getCollisionGroup();
   EXPECT_EQ(group->getNumShapeFrames(), 2u);
 
   // Default collision filter for Skeleton
-  CollisionOption option;
-  option.collisionFilter = std::make_shared<BodyNodeCollisionFilter>();
+  auto& option = constraintSolver->getCollisionOption();
+  auto bodyNodeFilter = std::make_shared<BodyNodeCollisionFilter>();
+  option.collisionFilter = bodyNodeFilter;
 
   skel->enableSelfCollisionCheck();
   skel->enableAdjacentBodyCheck();
@@ -1193,6 +1200,18 @@ void testFilter(const std::shared_ptr<CollisionDetector>& cd)
   EXPECT_FALSE(skel->isEnabledAdjacentBodyCheck());
   EXPECT_TRUE(group->collide());
   EXPECT_FALSE(group->collide(option));
+
+  // Test blacklist
+  skel->enableSelfCollisionCheck();
+  skel->enableAdjacentBodyCheck();
+  bodyNodeFilter->addBodyNodePairToBlackList(body0, body1);
+  EXPECT_FALSE(group->collide(option));
+  bodyNodeFilter->removeBodyNodePairFromBlackList(body0, body1);
+  EXPECT_TRUE(group->collide(option));
+  bodyNodeFilter->addBodyNodePairToBlackList(body0, body1);
+  EXPECT_FALSE(group->collide(option));
+  bodyNodeFilter->removeAllBodyNodePairsFromBlackList();
+  EXPECT_TRUE(group->collide(option));
 }
 
 //==============================================================================