diff --git a/trunk/fcl/include/fcl/articulated_model/joint.h b/trunk/fcl/include/fcl/articulated_model/joint.h
index 76db3d7b7e990f728d0bb00a950e3384c645c9c1..a484c26830c87df4e4e0435cf1523dec8720b4a6 100644
--- a/trunk/fcl/include/fcl/articulated_model/joint.h
+++ b/trunk/fcl/include/fcl/articulated_model/joint.h
@@ -45,78 +45,119 @@
 #include <map>
 #include <limits>
 #include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
 
 namespace fcl
 {
 
 class JointConfig;
+class Link;
+
+enum JointType {JT_UNKNOWN, JT_PRISMATIC, JT_REVOLUTE, JT_BALLEULER};
 
 /// @brief Base Joint
 class Joint
 {
 public:
-  Joint(const std::string& name, std::size_t dofs_num);
+
+  Joint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
+        const Transform3f& transform_to_parent,
+        const std::string& name);
+
+  Joint(const std::string& name);
 
   virtual ~Joint() {}
 
-  /// @brief Get joint's name
-  std::string getName() const;
+  const std::string& getName() const;
+  void setName(const std::string& name);
+
+  virtual Transform3f getLocalTransform() const = 0;
+
+  virtual std::size_t getNumDofs() const = 0;
+
+  boost::shared_ptr<JointConfig> getJointConfig() const;
+  void setJointConfig(const boost::shared_ptr<JointConfig>& joint_cfg);
+
+  boost::shared_ptr<Link> getParentLink() const;
+  boost::shared_ptr<Link> getChildLink() const;
+
+  void setParentLink(const boost::shared_ptr<Link>& link);
+  void setChildLink(const boost::shared_ptr<Link>& link);
+
+  JointType getJointType() const;
+
+  const Transform3f& getTransformToParent() const;
+  void setTransformToParent(const Transform3f& t);
+  
+protected:
 
-  /// @brief Get Joint's degrees of freedom
-  std::size_t getDOFs() const;
+  /// links to parent and child are only for connection, so weak_ptr to avoid cyclic dependency
+  boost::weak_ptr<Link> link_parent_, link_child_;
 
-  /// @brief How joint works
-  virtual Transform3f append(const JointConfig& q, const Transform3f& t) const = 0;
+  JointType type_;
 
-  bool isCompatible(const JointConfig& q) const;
+  std::string name_;
   
-private:
-  std::string name_;	
-  std::size_t dofs_num_;
+  boost::shared_ptr<JointConfig> joint_cfg_;
+
+  Transform3f transform_to_parent_;
 };
 
 
 class PrismaticJoint : public Joint
 {
 public:
-  PrismaticJoint(const std::string& name, const Vec3f& axis);
+  PrismaticJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
+                 const Transform3f& transform_to_parent,
+                 const std::string& name,
+                 const Vec3f& axis);
 
   virtual ~PrismaticJoint() {}
 
-  Vec3f getAxis() const;
+  Transform3f getLocalTransform() const;
 
-  Transform3f append(const JointConfig& q, const Transform3f& t) const;
+  std::size_t getNumDofs() const;
 
-private:
-  Vec3f axis_; // prismatic axis
-};
+  const Vec3f& getAxis() const;
 
+protected:
+  Vec3f axis_;
+};
 
 class RevoluteJoint : public Joint
 {
 public:
-  RevoluteJoint(const std::string& name, const Vec3f& axis);
+  RevoluteJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
+                const Transform3f& transform_to_parent,
+                const std::string& name,
+                const Vec3f& axis);
 
   virtual ~RevoluteJoint() {}
 
-  Vec3f getAxis() const;
+  Transform3f getLocalTransform() const;
 
-  Transform3f append(const JointConfig& q, const Transform3f& t) const;
-  
-private:
-  Vec3f axis_; // revolute axis
+  std::size_t getNumDofs() const;
+
+  const Vec3f& getAxis() const;
+
+protected:
+  Vec3f axis_;
 };
 
-class SphericJoint : public Joint
+
+
+class BallEulerJoint : public Joint
 {
 public:
-  SphericJoint(const std::string& name);
+  BallEulerJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
+                 const Transform3f& transform_to_parent,
+                 const std::string& name);
+
+  virtual ~BallEulerJoint() {}
 
-  virtual ~SphericJoint() {}
+  std::size_t getNumDofs() const;
 
-  Transform3f append(const JointConfig& q, const Transform3f& t) const;
- 
-private:
+  Transform3f getLocalTransform() const; 
 };
 
 
diff --git a/trunk/fcl/include/fcl/articulated_model/joint_config.h b/trunk/fcl/include/fcl/articulated_model/joint_config.h
index ecbdb95980e234e74fa3e44da412e6124af2c602..61d365eb4cabb767ac980d078ad85ba75c6086dd 100644
--- a/trunk/fcl/include/fcl/articulated_model/joint_config.h
+++ b/trunk/fcl/include/fcl/articulated_model/joint_config.h
@@ -39,6 +39,7 @@
 
 #include "fcl/data_types.h"
 #include <boost/shared_ptr.hpp>
+#include <boost/weak_ptr.hpp>
 #include <vector>
 
 namespace fcl
@@ -53,9 +54,12 @@ public:
 
   JointConfig(const JointConfig& joint_cfg);
 
-  JointConfig(boost::shared_ptr<Joint> joint, FCL_REAL default_value = 0);
+  JointConfig(const boost::shared_ptr<Joint>& joint,
+              FCL_REAL default_value = 0,
+              FCL_REAL default_value_min = 0,
+              FCL_REAL default_value_max = 0);
 
-  std::size_t size() const;
+  std::size_t getDim() const;
 
   inline FCL_REAL operator [] (std::size_t i) const
   {
@@ -67,14 +71,26 @@ public:
     return values_[i];
   }
 
-  bool operator == (const JointConfig& joint_cfg) const;
-
+  FCL_REAL getValue(std::size_t i) const;
+
+  FCL_REAL& getValue(std::size_t i);
+  
+  FCL_REAL getLimitMin(std::size_t i) const;
+  
+  FCL_REAL& getLimitMin(std::size_t i);
+  
+  FCL_REAL getLimitMax(std::size_t i) const;
+  
+  FCL_REAL& getLimitMax(std::size_t i);
+  
   boost::shared_ptr<Joint> getJoint() const;
 
 private:
-  boost::shared_ptr<Joint> joint_;
-
+  boost::weak_ptr<Joint> joint_;
+  
   std::vector<FCL_REAL> values_;
+  std::vector<FCL_REAL> limits_min_;
+  std::vector<FCL_REAL> limits_max_;
 };
 
 }
diff --git a/trunk/fcl/include/fcl/articulated_model/model.h b/trunk/fcl/include/fcl/articulated_model/model.h
index 9aa8bbddd379a1c7b5363d3e0860f2b707d6526d..102caa73ca799c2ab65e96d582534d6eca412e41 100644
--- a/trunk/fcl/include/fcl/articulated_model/model.h
+++ b/trunk/fcl/include/fcl/articulated_model/model.h
@@ -37,218 +37,61 @@
 #ifndef FCL_ARTICULATED_MODEL_MODEL_H
 #define FCL_ARTICULATED_MODEL_MODEL_H
 
-#include "fcl/data_types.h"
 #include "fcl/articulated_model/joint.h"
+#include "fcl/articulated_model/link.h"
 
-#include <map>
-
+#include "fcl/data_types.h"
 #include <boost/shared_ptr.hpp>
-#include <boost/weak_ptr.hpp>
-#include <boost/enable_shared_from_this.hpp>
-
 
+#include <map>
+#include <stdexcept>
 namespace fcl
 {
 
-
-template<typename Key, typename Value>
-class GeneralTreeNode : public boost::enable_shared_from_this<GeneralTreeNode<Key, Value> >
-{
-public:
-  GeneralTreeNode(Key key, Value value):
-    key_(key),
-    value_(value)
-  {}
-
-  void setParent(boost::weak_ptr<GeneralTreeNode<Key, Value> > parent)
-  {
-    this->parent_ = parent;
-
-    boost::shared_ptr<GeneralTreeNode<Key, Value> > parent_shared;
-
-    if(parent_shared = parent.lock())
-      parent_shared->addChild(this->shared_from_this());
-  }
-
-  boost::shared_ptr<GeneralTreeNode<Key, Value> > getParent() const
-  {
-    boost::shared_ptr<GeneralTreeNode<Key, Value> > parent_shared;
-    parent_shared = this->parent_.lock();
-    return parent_shared;
-  }
-
-  bool hasParent() const
-  {
-    bool has_parent = false;
-
-    if(this->parent_.lock())
-      has_parent = true;
-
-    return has_parent;
-  }
-
-  void addChild(boost::shared_ptr<GeneralTreeNode<Key, Value> > child)
-  {
-    this->children_map_[child->getKey()] = child;
-  }
-
-  std::map<Key, boost::shared_ptr<GeneralTreeNode<Key, Value> > > getChilds() const
-  {
-    return this->children_map_;
-  }
-
-  boost::shared_ptr<GeneralTreeNode<Key, Value> > getChildByKey(Key key) const
-  {
-    typename std::map<Key, boost::shared_ptr<GeneralTreeNode<Key, Value> > >::const_iterator it = this->children_map_.find(key);
-
-    boost::shared_ptr<GeneralTreeNode<Key, Value> > child = it->second; 
-
-    BOOST_ASSERT_MSG(child.use_count() != 0, "Child wasn't found! Maybe bad key.");
-
-    return child;
-  }
-
-  Key getKey() const
-  {
-    return this->key_;
-  }
-
-  Value getValue() const
-  {
-    return this->value_;
-  }
-
-private:
-  Key key_;
-  Value value_;
-
-  boost::weak_ptr<GeneralTreeNode<Key, Value> > parent_; // empty means that this Joint is root
-  std::map<Key, boost::shared_ptr<GeneralTreeNode<Key, Value> > > children_map_;
-};
-
-
-template<typename Key, typename Value>
-class GeneralTree
+class ModelParseError : public std::runtime_error
 {
 public:
-  GeneralTree()
-  {
-  }	
-
-  boost::shared_ptr<GeneralTreeNode<Key, Value> > createRoot(Key key, Value value)
-  {
-    boost::shared_ptr<GeneralTreeNode<Key, Value> > node(new GeneralTreeNode<Key, Value>(key,value));
-
-    if(this->root.use_count() != 0)
-    {
-      this->root->setParent(node);
-    }
-
-    this->root = node;
-    this->nodes_map[key] = node;
-
-    return node;
-  }
-
-  boost::shared_ptr<GeneralTreeNode<Key, Value> > createNode(Key key, Value value, Key parentKey)
-  {
-    boost::shared_ptr<GeneralTreeNode<Key, Value> > parent(getNodeByKey(parentKey) );
-
-    if (parent.use_count() == 0)
-    {
-      return boost::shared_ptr<GeneralTreeNode<Key, Value> >();
-    }
-
-    boost::shared_ptr<GeneralTreeNode<Key, Value> > node(new GeneralTreeNode<Key, Value>(key,value) );
-
-    node->setParent(parent);
-    this->nodes_map[key] = node;
-
-    return node;
-  }	
-
-  boost::shared_ptr<GeneralTreeNode<Key, Value> > getNodeByKey(Key key) const
-  {
-    typename std::map<Key, boost::weak_ptr<GeneralTreeNode<Key, Value> > >::const_iterator it = nodes_map.find(key);
-    boost::weak_ptr<GeneralTreeNode<Key, Value> > node = it->second;
-    return node.lock();
-  }
-
-  bool existsKey(Key key) const
-  {
-    return nodes_map.find(key) != nodes_map.end();
-  }
-
-  FCL_INT32 getNodesNum() const
-  {
-    return this->nodes_map.size();
-  }
-
-  std::map<Key, Value> getValuesMap() const
-  {
-    std::map<Key, Value> values_map;
-
-    typename std::map<Key, boost::weak_ptr<GeneralTreeNode<Key, Value> > >::const_iterator it;
-
-    for(it = this->nodes_map.begin(); it != this->nodes_map.end(); ++it)
-    {
-      Key first = it->first;
-      if(boost::shared_ptr<GeneralTreeNode<Key, Value> > second_shared = it->second.lock())
-      {
-        values_map[first] = second_shared->getValue();
-      }
-    }
-
-    return values_map;
-  }
-
-private:
-
-  boost::shared_ptr<GeneralTreeNode<Key, Value> > root;
-  std::map<Key, boost::weak_ptr<GeneralTreeNode<Key, Value> > > nodes_map; // holds node under its key
+  ModelParseError(const std::string& error_msg) : std::runtime_error(error_msg) {}
 };
 
-class ModelConfig;
-
-class Model : public boost::enable_shared_from_this<Model>
+class Model
 {
 public:
-  Model(const std::string& name);
-
-  boost::shared_ptr<Model> getSharedPtr();
+  Model() {}
+  
+  virtual ~Model() {}
 
-  std::string getName() const;
+  const std::string& getName() const;
+  
+  void addLink(const boost::shared_ptr<Link>& link);
 
-  void addJoint(boost::shared_ptr<Joint> joint, const std::string& parent_name = "");
+  void addJoint(const boost::shared_ptr<Joint>& joint);
 
-  boost::shared_ptr<Joint> getJointByName(const std::string& joint_name) const;
-  boost::shared_ptr<Joint> getJointParentByName(const std::string& joint_name) const;
-  boost::shared_ptr<Joint> getJointParent(boost::shared_ptr<Joint> joint) const;
+  void initRoot(const std::map<std::string, std::string>& link_parent_tree);
 
-  bool hasJointParentByName(const std::string& joint_name) const;
-  bool hasJointParent(boost::shared_ptr<Joint> joint) const;
+  void initTree(std::map<std::string, std::string>& link_parent_tree);
 
-  std::size_t getJointNum() const;
+  std::size_t getNumDofs() const;
 
-  std::map<std::string, boost::shared_ptr<Joint> > getJointsMap() const;
+  std::size_t getNumLinks() const;
 
-  ///FCL_REAL getMotionBoundFromParentToChildJoint(const std::string& child_name) const;
-  ///FCL_REAL getMotionBoundFromParentToChildJoint(boost::shared_ptr<Joint> child_joint) const;
+  std::size_t getNumJoints() const;
+  
+  boost::shared_ptr<Link> getRoot() const;
+  boost::shared_ptr<Link> getLink(const std::string& name) const;
+  boost::shared_ptr<Joint> getJoint(const std::string& name) const;
 
-  ///Vec3f getVectorFromParentToChildJoint(const std::string& child_name, FCL_REAL time) const;
-  ///Vec3f getVectorFromParentToChildJoint(boost::shared_ptr<Joint> child_joint, FCL_REAL time) const;
+  std::vector<boost::shared_ptr<Link> > getLinks() const;
+  std::vector<boost::shared_ptr<Joint> > getJoints() const;
+protected:
+  boost::shared_ptr<Link> root_link_;
+  std::map<std::string, boost::shared_ptr<Link> > links_;
+  std::map<std::string, boost::shared_ptr<Joint> > joints_;
 
-  std::vector<boost::shared_ptr<Joint> > getJointsChain(const std::string& last_joint_name) const;
-  std::vector<boost::shared_ptr<Joint> > getJointsChain(boost::shared_ptr<Joint> last_joint) const;
-
-  bool isCompatible(const ModelConfig& model_config) const;
-
-private:
   std::string name_;
-  GeneralTree<std::string, boost::shared_ptr<Joint> > joints_tree_;
+  
 };
 
-
 }
 
 #endif
diff --git a/trunk/fcl/include/fcl/articulated_model/model_config.h b/trunk/fcl/include/fcl/articulated_model/model_config.h
index 9e7c99bca71a5e7da0f52ebe3d08e22fd0e6a0c0..fdd7c50d63f9a186a8e0d37d6c08d1b0650be776 100644
--- a/trunk/fcl/include/fcl/articulated_model/model_config.h
+++ b/trunk/fcl/include/fcl/articulated_model/model_config.h
@@ -62,8 +62,6 @@ public:
   JointConfig getJointConfigByJoint(boost::shared_ptr<Joint> joint) const;
   JointConfig& getJointConfigByJoint(boost::shared_ptr<Joint> joint);
 
-  bool operator == (const ModelConfig& model_cfg) const;
-
   std::map<std::string, JointConfig> getJointCfgsMap() const
   { return joint_cfgs_map_; }
 
diff --git a/trunk/fcl/include/fcl/ccd/interpolation/interpolation.h b/trunk/fcl/include/fcl/ccd/interpolation/interpolation.h
index d52538f0fbe1a000d597c4326c6cda2ded723c43..4148b8ad838e72f4d61985ce425a9bdcdb140761 100644
--- a/trunk/fcl/include/fcl/ccd/interpolation/interpolation.h
+++ b/trunk/fcl/include/fcl/ccd/interpolation/interpolation.h
@@ -1,3 +1,39 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  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.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  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 OWNER 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.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
 #ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_H
 #define FCL_CCD_INTERPOLATION_INTERPOLATION_H
 
diff --git a/trunk/fcl/include/fcl/ccd/interpolation/interpolation_factory.h b/trunk/fcl/include/fcl/ccd/interpolation/interpolation_factory.h
index 2729231a8d4d66a2c7c41d1f3e4039fc7c3d85dc..08f907fd1722c7e7250ea05fe23191ef2e6ec864 100644
--- a/trunk/fcl/include/fcl/ccd/interpolation/interpolation_factory.h
+++ b/trunk/fcl/include/fcl/ccd/interpolation/interpolation_factory.h
@@ -1,3 +1,39 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  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.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  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 OWNER 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.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
 #ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_FACTORY_H
 #define FCL_CCD_INTERPOLATION_INTERPOLATION_FACTORY_H
 
@@ -42,4 +78,4 @@ private:
 
 }
 
-#endif /* #ifndef FCL_INTERPOLATION_FACTORY_H */
+#endif
diff --git a/trunk/fcl/include/fcl/ccd/interpolation/interpolation_linear.h b/trunk/fcl/include/fcl/ccd/interpolation/interpolation_linear.h
index 92f35a0573f6bd54520d2b91f6bcf3de070bf1cb..e15252b3469d8b0f7f7a073319fece8d4898306f 100644
--- a/trunk/fcl/include/fcl/ccd/interpolation/interpolation_linear.h
+++ b/trunk/fcl/include/fcl/ccd/interpolation/interpolation_linear.h
@@ -1,3 +1,39 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  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.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  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 OWNER 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.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
 #ifndef FCL_CCD_INTERPOLATION_INTERPOLATION_LINEAR_H
 #define FCL_CCD_INTERPOLATION_INTERPOLATION_LINEAR_H
 
diff --git a/trunk/fcl/include/fcl/ccd/interval.h b/trunk/fcl/include/fcl/ccd/interval.h
index e6e96b69fee52aa59206c2730018337433077367..4d554f615039cbc0e60fa8ff7b9aed37ea58e27d 100644
--- a/trunk/fcl/include/fcl/ccd/interval.h
+++ b/trunk/fcl/include/fcl/ccd/interval.h
@@ -35,8 +35,8 @@
 /** \author Jia Pan */
 
 
-#ifndef FCL_INTERVAL_H
-#define FCL_INTERVAL_H
+#ifndef FCL_CCD_INTERVAL_H
+#define FCL_CCD_INTERVAL_H
 
 #include "fcl/data_types.h"
 
diff --git a/trunk/fcl/include/fcl/ccd/interval_matrix.h b/trunk/fcl/include/fcl/ccd/interval_matrix.h
index a6fe5ab5924e9b91f37f43e1d20745bf8ce2690b..a130d499fd75821eeb1c6fc4cb8a838c86510bb9 100644
--- a/trunk/fcl/include/fcl/ccd/interval_matrix.h
+++ b/trunk/fcl/include/fcl/ccd/interval_matrix.h
@@ -35,8 +35,8 @@
 /** \author Jia Pan */
 
 
-#ifndef FCL_INTERVAL_MATRIX_H
-#define FCL_INTERVAL_MATRIX_H
+#ifndef FCL_CCD_INTERVAL_MATRIX_H
+#define FCL_CCD_INTERVAL_MATRIX_H
 
 #include "fcl/ccd/interval.h"
 #include "fcl/ccd/interval_vector.h"
@@ -95,10 +95,13 @@ struct IMatrix3
   IMatrix3& operator *= (const IMatrix3& m);
   IMatrix3& operator *= (const Matrix3f& m);
 
+  IMatrix3& rotationConstrain();
 
   void print() const;
 };
 
+IMatrix3 rotationConstrain(const IMatrix3& m);
+
 }
 
 #endif
diff --git a/trunk/fcl/include/fcl/ccd/interval_vector.h b/trunk/fcl/include/fcl/ccd/interval_vector.h
index 2a60dc4c97d83dfb98a01d27a224a898ae275fb6..1249c5ffde2b8a35cbefb8711dd977c663bfeaae 100644
--- a/trunk/fcl/include/fcl/ccd/interval_vector.h
+++ b/trunk/fcl/include/fcl/ccd/interval_vector.h
@@ -35,8 +35,8 @@
 /** \author Jia Pan */
 
 
-#ifndef FCL_INTERVAL_VECTOR_H
-#define FCL_INTERVAL_VECTOR_H
+#ifndef FCL_CCD_INTERVAL_VECTOR_H
+#define FCL_CCD_INTERVAL_VECTOR_H
 
 #include "fcl/ccd/interval.h"
 #include "fcl/math/vec_3f.h"
diff --git a/trunk/fcl/include/fcl/ccd/motion.h b/trunk/fcl/include/fcl/ccd/motion.h
index 0fd9f91703ff796b77f4221f95f9f6d85fc014aa..a54b9c16212d561d001d4d33655fd5932ed51646 100644
--- a/trunk/fcl/include/fcl/ccd/motion.h
+++ b/trunk/fcl/include/fcl/ccd/motion.h
@@ -35,8 +35,8 @@
 /** \author Jia Pan */
 
 
-#ifndef FCL_MOTION_H
-#define FCL_MOTION_H
+#ifndef FCL_CCD_MOTION_H
+#define FCL_CCD_MOTION_H
 
 #include "fcl/ccd/motion_base.h"
 #include "fcl/intersect.h"
diff --git a/trunk/fcl/include/fcl/ccd/motion_base.h b/trunk/fcl/include/fcl/ccd/motion_base.h
index bfd6176379da6c4a41d6425cab50f2ee41b920d4..50f641ae0f2386ad4a24412d1765573a0fc34c53 100644
--- a/trunk/fcl/include/fcl/ccd/motion_base.h
+++ b/trunk/fcl/include/fcl/ccd/motion_base.h
@@ -35,8 +35,8 @@
 /** \author Jia Pan */
 
 
-#ifndef FCL_MOTION_BASE_H
-#define FCL_MOTION_BASE_H
+#ifndef FCL_CCD_MOTION_BASE_H
+#define FCL_CCD_MOTION_BASE_H
 
 
 #include "fcl/math/transform.h"
diff --git a/trunk/fcl/include/fcl/ccd/taylor_matrix.h b/trunk/fcl/include/fcl/ccd/taylor_matrix.h
index 0c02a3bf352fd8a039e86f3359239ad7673ea581..255b095b776dc24a771c1d5338070a9b3d666f16 100644
--- a/trunk/fcl/include/fcl/ccd/taylor_matrix.h
+++ b/trunk/fcl/include/fcl/ccd/taylor_matrix.h
@@ -34,8 +34,8 @@
 
 /** \author Jia Pan */
 
-#ifndef FCL_TAYLOR_MATRIX_H
-#define FCL_TAYLOR_MATRIX_H
+#ifndef FCL_CCD_TAYLOR_MATRIX_H
+#define FCL_CCD_TAYLOR_MATRIX_H
 
 
 #include "fcl/math/matrix_3f.h"
@@ -86,8 +86,12 @@ struct TMatrix3
   FCL_REAL diameter() const;
 
   void setTimeInterval(const boost::shared_ptr<TimeInterval>& time_interval);
+
+  TMatrix3& rotationConstrain();
 };
 
+TMatrix3 rotationConstrain(const TMatrix3& m);
+
 }
 
 #endif
diff --git a/trunk/fcl/include/fcl/ccd/taylor_model.h b/trunk/fcl/include/fcl/ccd/taylor_model.h
index 9012de5a16541431f102993ed4194c8bc485b803..f7c2b037b568101314438cd459a80484bf2c3c4f 100644
--- a/trunk/fcl/include/fcl/ccd/taylor_model.h
+++ b/trunk/fcl/include/fcl/ccd/taylor_model.h
@@ -34,8 +34,8 @@
 
 /** \author Jia Pan */
 
-#ifndef FCL_TAYLOR_MODEL_H
-#define FCL_TAYLOR_MODEL_H
+#ifndef FCL_CCD_TAYLOR_MODEL_H
+#define FCL_CCD_TAYLOR_MODEL_H
 
 #include "fcl/ccd/interval.h"
 #include <boost/shared_ptr.hpp>
diff --git a/trunk/fcl/include/fcl/ccd/taylor_vector.h b/trunk/fcl/include/fcl/ccd/taylor_vector.h
index 8771feec8a6c2ec9e0597ebbcfac687a06d8f17d..c930e62ff3502f7036fd5074e7894b1b36f10882 100644
--- a/trunk/fcl/include/fcl/ccd/taylor_vector.h
+++ b/trunk/fcl/include/fcl/ccd/taylor_vector.h
@@ -34,8 +34,8 @@
 
 /** \author Jia Pan */
 
-#ifndef FCL_TAYLOR_VECTOR_H
-#define FCL_TAYLOR_VECTOR_H
+#ifndef FCL_CCD_TAYLOR_VECTOR_H
+#define FCL_CCD_TAYLOR_VECTOR_H
 
 #include "fcl/ccd/interval_vector.h"
 #include "fcl/ccd/taylor_model.h"
diff --git a/trunk/fcl/src/CMakeLists.txt b/trunk/fcl/src/CMakeLists.txt
index 368da586a5d9d93eb5932087ac0d8384006cc887..4f69c9a1d77badc7069b47d0fd40207cba6ead1b 100644
--- a/trunk/fcl/src/CMakeLists.txt
+++ b/trunk/fcl/src/CMakeLists.txt
@@ -44,6 +44,7 @@ add_library(${PROJECT_NAME} SHARED
   broadphase/hierarchy_tree.cpp
   articulated_model/joint_config.cpp
   articulated_model/joint.cpp
+  articulated_model/link.cpp
   articulated_model/model_config.cpp
   articulated_model/model.cpp
   ccd/interpolation/interpolation.cpp
diff --git a/trunk/fcl/src/articulated_model/joint.cpp b/trunk/fcl/src/articulated_model/joint.cpp
index 8d42376769ac8b1706045246f6375e119a662962..2243f0815d396de302a2ea5ed1a4ea89fc86e6f8 100644
--- a/trunk/fcl/src/articulated_model/joint.cpp
+++ b/trunk/fcl/src/articulated_model/joint.cpp
@@ -35,81 +35,153 @@
 /** \author Dalibor Matura, Jia Pan */
 
 #include "fcl/articulated_model/joint.h"
+#include "fcl/articulated_model/link.h"
 #include "fcl/articulated_model/joint_config.h"
 
 namespace fcl
 {
 
-Joint::Joint(const std::string& name, std::size_t dofs_num) :
-  name_(name),
-  dofs_num_(dofs_num)
+Joint::Joint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
+             const Transform3f& transform_to_parent,
+             const std::string& name) :
+  link_parent_(link_parent), link_child_(link_child),
+  transform_to_parent_(transform_to_parent),
+  name_(name)
+{}
+
+Joint::Joint(const std::string& name) :
+  name_(name)
 {
 }
 
-std::string Joint::getName() const
+const std::string& Joint::getName() const
 {
   return name_;
 }
 
-std::size_t Joint::getDOFs() const
+void Joint::setName(const std::string& name)
+{
+  name_ = name;
+}
+
+boost::shared_ptr<JointConfig> Joint::getJointConfig() const
+{
+  return joint_cfg_;
+}
+
+void Joint::setJointConfig(const boost::shared_ptr<JointConfig>& joint_cfg)
+{
+  joint_cfg_ = joint_cfg;
+}
+
+boost::shared_ptr<Link> Joint::getParentLink() const
+{
+  return link_parent_.lock();
+}
+
+boost::shared_ptr<Link> Joint::getChildLink() const
+{
+  return link_child_.lock();
+}
+
+void Joint::setParentLink(const boost::shared_ptr<Link>& link)
+{
+  link_parent_ = link;
+}
+
+void Joint::setChildLink(const boost::shared_ptr<Link>& link)
+{
+  link_child_ = link;
+}
+
+JointType Joint::getJointType() const
+{
+  return type_;
+}
+
+const Transform3f& Joint::getTransformToParent() const
 {
-  return dofs_num_;
+  return transform_to_parent_;
 }
 
-bool Joint::isCompatible(const JointConfig& q) const
+void Joint::setTransformToParent(const Transform3f& t)
 {
-  return this == q.getJoint().get();
+  transform_to_parent_ = t;
 }
 
-PrismaticJoint::PrismaticJoint(const std::string& name, const Vec3f& axis) :
-  Joint(name, 1),
+
+PrismaticJoint::PrismaticJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
+                               const Transform3f& transform_to_parent,
+                               const std::string& name,
+                               const Vec3f& axis) :
+  Joint(link_parent, link_child, transform_to_parent, name),
   axis_(axis)
 {
-  axis_.normalize();
+  type_ = JT_PRISMATIC;
 }
 
-Vec3f PrismaticJoint::getAxis() const
+const Vec3f& PrismaticJoint::getAxis() const
 {
   return axis_;
 }
 
-Transform3f PrismaticJoint::append(const JointConfig& q, const Transform3f& t) const
+std::size_t PrismaticJoint::getNumDofs() const
 {
-  BOOST_ASSERT(isCompatible(q));
-  /// axis_ is in local frame
-  return Transform3f(axis_ * q[0]) * t; 
+  return 1;
 }
 
-RevoluteJoint::RevoluteJoint(const std::string& name, const Vec3f& axis) :
-  Joint(name, 1),
+Transform3f PrismaticJoint::getLocalTransform() const
+{
+  const Quaternion3f& quat = transform_to_parent_.getQuatRotation();
+  const Vec3f& transl = transform_to_parent_.getTranslation();
+  return Transform3f(quat, quat.transform(axis_ * (*joint_cfg_)[0]) + transl);
+}
+
+
+RevoluteJoint::RevoluteJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
+                             const Transform3f& transform_to_parent,
+                             const std::string& name,
+                             const Vec3f& axis) :
+  Joint(link_parent, link_child, transform_to_parent, name),
   axis_(axis)
 {
-  axis_.normalize();
+  type_ = JT_REVOLUTE;
 }
 
-Vec3f RevoluteJoint::getAxis() const
+const Vec3f& RevoluteJoint::getAxis() const
 {
   return axis_;
 }
 
-Transform3f RevoluteJoint::append(const JointConfig& q, const Transform3f& t) const
+std::size_t RevoluteJoint::getNumDofs() const
+{
+  return 1;
+}
+
+Transform3f RevoluteJoint::getLocalTransform() const
 {
-  BOOST_ASSERT(isCompatible(q));
   Quaternion3f quat;
-  quat.fromAxisAngle(axis_, q[0]);
-  return Transform3f(quat) * t;
+  quat.fromAxisAngle(axis_, (*joint_cfg_)[0]);
+  return Transform3f(transform_to_parent_.getQuatRotation() * quat, transform_to_parent_.getTranslation());
 }
 
-SphericJoint::SphericJoint(const std::string& name) :
-  Joint(name, 3)
+
+BallEulerJoint::BallEulerJoint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
+                               const Transform3f& transform_to_parent,
+                               const std::string& name) :
+  Joint(link_parent, link_child, transform_to_parent, name)
 {}
 
-Transform3f SphericJoint::append(const JointConfig& q, const Transform3f& t) const
+std::size_t BallEulerJoint::getNumDofs() const
+{
+  return 3;
+}
+
+Transform3f BallEulerJoint::getLocalTransform() const
 {
-  BOOST_ASSERT(isCompatible(q));
   Matrix3f rot;
-  rot.setEulerYPR(q[0], q[1], q[2]);
-  return Transform3f(rot) * t;
+  rot.setEulerYPR((*joint_cfg_)[0], (*joint_cfg_)[1], (*joint_cfg_)[2]);
+  return transform_to_parent_ * Transform3f(rot);
 }
 
 
diff --git a/trunk/fcl/src/articulated_model/joint_config.cpp b/trunk/fcl/src/articulated_model/joint_config.cpp
index bca2438adcd1752c6f78406ece6fa605249a3bcc..5f6c807e9415f924cb804d3c9836147d8d4c6eef 100644
--- a/trunk/fcl/src/articulated_model/joint_config.cpp
+++ b/trunk/fcl/src/articulated_model/joint_config.cpp
@@ -44,30 +44,63 @@ JointConfig::JointConfig() {}
 
 JointConfig::JointConfig(const JointConfig& joint_cfg) :
   joint_(joint_cfg.joint_),
-  values_(joint_cfg.values_)
+  values_(joint_cfg.values_),
+  limits_min_(joint_cfg.limits_min_),
+  limits_max_(joint_cfg.limits_max_)
 {
 }
 
-JointConfig::JointConfig(boost::shared_ptr<Joint> joint, FCL_REAL default_value) :
+JointConfig::JointConfig(const boost::shared_ptr<Joint>& joint,
+                         FCL_REAL default_value,
+                         FCL_REAL default_value_min,
+                         FCL_REAL default_value_max) :
   joint_(joint)
 {
-  values_.resize(joint->getDOFs(), default_value);
+  values_.resize(joint->getNumDofs(), default_value);
+  limits_min_.resize(joint->getNumDofs(), default_value_min);
+  limits_max_.resize(joint->getNumDofs(), default_value_max);
 }
 
-std::size_t JointConfig::size() const
+std::size_t JointConfig::getDim() const
 {
   return values_.size();
 }
 
-bool JointConfig::operator == (const JointConfig& joint_cfg) const
+FCL_REAL JointConfig::getValue(std::size_t i) const
 {
-  return (joint_ == joint_cfg.joint_) &&
-    std::equal(values_.begin(), values_.end(), joint_cfg.values_.begin());
+  return values_[i];
 }
 
+FCL_REAL& JointConfig::getValue(std::size_t i)
+{
+  return values_[i];
+}
+
+FCL_REAL JointConfig::getLimitMin(std::size_t i) const
+{
+  return limits_min_[i];
+}
+
+FCL_REAL& JointConfig::getLimitMin(std::size_t i)
+{
+  return limits_min_[i];
+}
+
+FCL_REAL JointConfig::getLimitMax(std::size_t i) const
+{
+  return limits_max_[i];
+}
+
+FCL_REAL& JointConfig::getLimitMax(std::size_t i)
+{
+  return limits_max_[i];
+}
+
+
+
 boost::shared_ptr<Joint> JointConfig::getJoint() const
 {
-  return joint_;
+  return joint_.lock();
 }
 
 }
diff --git a/trunk/fcl/src/articulated_model/model.cpp b/trunk/fcl/src/articulated_model/model.cpp
index f234b2458eca9e9aacedbd6d937c2362628e1783..fd45147a8be549d79ad9187fec6846d2e6174335 100644
--- a/trunk/fcl/src/articulated_model/model.cpp
+++ b/trunk/fcl/src/articulated_model/model.cpp
@@ -41,105 +41,118 @@
 namespace fcl
 {
 
-Model::Model(const std::string& name) :
-  name_(name)
-{
-}
 
-boost::shared_ptr<Model> Model::getSharedPtr()
+boost::shared_ptr<Link> Model::getRoot() const
 {
-  return shared_from_this();
+  return root_link_;
 }
 
-std::string Model::getName() const
+boost::shared_ptr<Link> Model::getLink(const std::string& name) const
 {
-  return name_;
+  boost::shared_ptr<Link> ptr;
+  std::map<std::string, boost::shared_ptr<Link> >::const_iterator it = links_.find(name);
+  if(it == links_.end())
+    ptr.reset();
+  else
+    ptr = it->second;
+  return ptr;
 }
 
-void Model::addJoint(boost::shared_ptr<Joint> joint, const std::string& parent_name)
+boost::shared_ptr<Joint> Model::getJoint(const std::string& name) const
 {
-  if(parent_name == "")
-    joints_tree_.createRoot(joint->getName(), joint);
+  boost::shared_ptr<Joint> ptr;
+  std::map<std::string, boost::shared_ptr<Joint> >::const_iterator it = joints_.find(name);
+  if(it == joints_.end())
+    ptr.reset();
   else
-    joints_tree_.createNode(joint->getName(), joint, parent_name);
+    ptr = it->second;
+  return ptr;
 }
 
-boost::shared_ptr<Joint> Model::getJointByName(const std::string& joint_name) const
+const std::string& Model::getName() const
 {
-  boost::shared_ptr<GeneralTreeNode<std::string, boost::shared_ptr<Joint> > > node = joints_tree_.getNodeByKey(joint_name);
-
-  return node->getValue();
+  return name_;
 }
 
-boost::shared_ptr<Joint> Model::getJointParentByName(const std::string& joint_name) const
+std::vector<boost::shared_ptr<Link> > Model::getLinks() const
 {
-  boost::shared_ptr<GeneralTreeNode<std::string, boost::shared_ptr<Joint> > > node = joints_tree_.getNodeByKey(joint_name);
-
-  boost::shared_ptr<GeneralTreeNode<std::string, boost::shared_ptr<Joint> > > parent_node = node->getParent();
+  std::vector<boost::shared_ptr<Link> > links;
+  for(std::map<std::string, boost::shared_ptr<Link> >::const_iterator it = links_.begin(); it != links_.end(); ++it)
+  {
+    links.push_back(it->second);
+  }
 
-  if(parent_node.use_count() == 0)
-    return boost::shared_ptr<Joint>();
-  else
-    return parent_node->getValue();
+  return links;
 }
 
-boost::shared_ptr<Joint> Model::getJointParent(boost::shared_ptr<Joint> joint) const
+std::size_t Model::getNumLinks() const
 {
-  return getJointParentByName(joint->getName());
+  return links_.size();
 }
 
-bool Model::hasJointParentByName(const std::string& joint_name) const
+std::size_t Model::getNumJoints() const
 {
-  boost::shared_ptr<GeneralTreeNode<std::string, boost::shared_ptr<Joint> > > node = joints_tree_.getNodeByKey(joint_name);
-
-  return node->hasParent();
+  return joints_.size();
 }
 
-bool Model::hasJointParent(boost::shared_ptr<Joint> joint) const
+std::size_t Model::getNumDofs() const
 {
-  return hasJointParentByName(joint->getName());
+  std::size_t dof = 0;
+  for(std::map<std::string, boost::shared_ptr<Joint> >::const_iterator it = joints_.begin(); it != joints_.end(); ++it)
+  {
+    dof += it->second->getNumDofs();
+  }
+
+  return dof;
 }
 
-std::size_t Model::getJointNum() const
+void Model::addLink(const boost::shared_ptr<Link>& link)
 {
-  return joints_tree_.getNodesNum();
+  links_[link->getName()] = link;
 }
 
-std::map<std::string, boost::shared_ptr<Joint> > Model::getJointsMap() const
+void Model::addJoint(const boost::shared_ptr<Joint>& joint)
 {
-  return joints_tree_.getValuesMap();
+  joints_[joint->getName()] = joint;
 }
 
-bool Model::isCompatible(const ModelConfig& model_config) const
+void Model::initRoot(const std::map<std::string, std::string>& link_parent_tree)
 {
-  std::map<std::string, JointConfig> joint_cfgs_map = model_config.getJointCfgsMap();
-  std::map<std::string, boost::shared_ptr<Joint> > joints_map = getJointsMap();
-
-  std::map<std::string, boost::shared_ptr<Joint> >::const_iterator it;
+  root_link_.reset();
 
-  for(it = joints_map.begin(); it != joints_map.end(); ++it)
+  /// find the links that have no parent in the tree
+  for(std::map<std::string, boost::shared_ptr<Link> >::const_iterator it = links_.begin(); it != links_.end(); ++it)
   {
-    std::map<std::string, JointConfig>::const_iterator it2 = joint_cfgs_map.find(it->first);
-    if(it2 == joint_cfgs_map.end()) return false;
-
-    if(it2->second.getJoint() != it->second) return false;
+    std::map<std::string, std::string>::const_iterator parent = link_parent_tree.find(it->first);
+    if(parent == link_parent_tree.end())
+    {
+      if(!root_link_)
+      {
+        root_link_ = getLink(it->first);
+      }
+      else
+      {
+        throw ModelParseError("Two root links found: [" + root_link_->getName() + "] and [" + it->first + "]");
+      }
+    }
   }
 
-  return true;
+  if(!root_link_)
+    throw ModelParseError("No root link found.");
 }
 
-std::vector<boost::shared_ptr<Joint> > Model::getJointsChain(const std::string& last_joint_name) const
+void Model::initTree(std::map<std::string, std::string>& link_parent_tree)
 {
-  std::vector<boost::shared_ptr<Joint> > chain;
-  boost::shared_ptr<Joint> joint = getJointByName(last_joint_name);
-
-  while(joint.use_count() != 0)
+  for(std::map<std::string, boost::shared_ptr<Joint> >::iterator it = joints_.begin(); it != joints_.end(); ++it)
   {
-    chain.push_back(joint);
-    joint = getJointParent(joint);
-  }
+    std::string parent_link_name = it->second->getParentLink()->getName();
+    std::string child_link_name = it->second->getChildLink()->getName();
 
-  return chain;
+    it->second->getParentLink()->addChildJoint(it->second);
+    it->second->getChildLink()->setParentJoint(it->second);
+
+    link_parent_tree[child_link_name] = parent_link_name;
+  }
 }
 
 }
diff --git a/trunk/fcl/src/articulated_model/model_config.cpp b/trunk/fcl/src/articulated_model/model_config.cpp
index 07e7864ee8edae7cabb1e8aa889cbf7f97151a38..43d62de641dd994f7d923c762cabb9e58e9eee55 100644
--- a/trunk/fcl/src/articulated_model/model_config.cpp
+++ b/trunk/fcl/src/articulated_model/model_config.cpp
@@ -80,10 +80,5 @@ JointConfig& ModelConfig::getJointConfigByJoint(boost::shared_ptr<Joint> joint)
   return getJointConfigByJointName(joint->getName());
 }
 
-bool ModelConfig::operator == (const ModelConfig& model_cfg) const
-{
-  return (joint_cfgs_map_.size() == model_cfg.joint_cfgs_map_.size()) &&
-    std::equal(joint_cfgs_map_.begin(), joint_cfgs_map_.end(), model_cfg.joint_cfgs_map_.begin());
-}
 
 }
diff --git a/trunk/fcl/src/ccd/interpolation/interpolation.cpp b/trunk/fcl/src/ccd/interpolation/interpolation.cpp
index 7305a1dd456fc72568220f7afe8e9354bcde78f0..7639e3b3958d501a2cd2825d2a9f19a2a8262155 100644
--- a/trunk/fcl/src/ccd/interpolation/interpolation.cpp
+++ b/trunk/fcl/src/ccd/interpolation/interpolation.cpp
@@ -1,3 +1,39 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  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.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  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 OWNER 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.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
 #include "fcl/ccd/interpolation/interpolation.h"
 
 namespace fcl 
diff --git a/trunk/fcl/src/ccd/interpolation/interpolation_factory.cpp b/trunk/fcl/src/ccd/interpolation/interpolation_factory.cpp
index 1ec4fd831d5ec53920ca465db49dc99b4476ba7f..c9f81ee2b23fcbdeb56964efd7e42b6ef3324e84 100644
--- a/trunk/fcl/src/ccd/interpolation/interpolation_factory.cpp
+++ b/trunk/fcl/src/ccd/interpolation/interpolation_factory.cpp
@@ -1,3 +1,39 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  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.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  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 OWNER 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.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
 #include "fcl/ccd/interpolation/interpolation_factory.h"
 #include "fcl/ccd/interpolation/interpolation_linear.h"
 
diff --git a/trunk/fcl/src/ccd/interpolation/interpolation_linear.cpp b/trunk/fcl/src/ccd/interpolation/interpolation_linear.cpp
index 8ae0f2c97ba3951b23c38ca4efa1c7e436ef00eb..4405f7831e39c5e8158068086fca0801c14867a1 100644
--- a/trunk/fcl/src/ccd/interpolation/interpolation_linear.cpp
+++ b/trunk/fcl/src/ccd/interpolation/interpolation_linear.cpp
@@ -1,3 +1,39 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2011, Willow Garage, Inc.
+ *  All rights reserved.
+ *
+ *  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.
+ *   * Neither the name of Willow Garage, Inc. nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  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 OWNER 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.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
 #include "fcl/ccd/interpolation/interpolation_linear.h"
 #include "fcl/ccd/interpolation/interpolation_factory.h"
 
diff --git a/trunk/fcl/src/ccd/interval_matrix.cpp b/trunk/fcl/src/ccd/interval_matrix.cpp
index 442b28589235921a560d4b72eb25ced2ef0b1638..c4739624d262280efb2cfb819af07833bfa8186e 100644
--- a/trunk/fcl/src/ccd/interval_matrix.cpp
+++ b/trunk/fcl/src/ccd/interval_matrix.cpp
@@ -218,6 +218,23 @@ IMatrix3& IMatrix3::operator -= (const IMatrix3& m)
   return *this;
 }
 
+IMatrix3& IMatrix3::rotationConstrain()
+{
+  for(std::size_t i = 0; i < 3; ++i)
+  {
+    for(std::size_t j = 0; j < 3; ++j)
+    {
+      if(v_[i][j][0] < -1) v_[i][j][0] = -1;
+      else if(v_[i][j][0] > 1) v_[i][j][0] = 1;
+
+      if(v_[i][j][1] < -1) v_[i][j][1] = -1;
+      else if(v_[i][j][1] > 1) v_[i][j][1] = 1;
+    }
+  }
+
+  return *this;
+}
+
 void IMatrix3::print() const
 {
   std::cout << "[" << v_[0][0][0] << "," << v_[0][0][1] << "]" << " [" << v_[0][1][0] << "," << v_[0][1][1] << "]" << " [" << v_[0][2][0] << "," << v_[0][2][1] << "]" << std::endl;
@@ -225,4 +242,22 @@ void IMatrix3::print() const
   std::cout << "[" << v_[2][0][0] << "," << v_[2][0][1] << "]" << " [" << v_[2][1][0] << "," << v_[2][1][1] << "]" << " [" << v_[2][2][0] << "," << v_[2][2][1] << "]" << std::endl;
 }
 
+IMatrix3 rotationConstrain(const IMatrix3& m)
+{
+  IMatrix3 res;
+  for(std::size_t i = 0; i < 3; ++i)
+  {
+    for(std::size_t j = 0; j < 3; ++j)
+    {
+      if(m(i, j)[0] < -1) res(i, j)[0] = -1;
+      else if(m(i, j)[0] > 1) res(i, j)[0] = 1;
+
+      if(m(i, j)[1] < -1) res(i, j)[1] = -1;
+      else if(m(i, j)[1] > 1) res(i, j)[1] = 1;      
+    }
+  }
+
+  return res;
+}
+
 }
diff --git a/trunk/fcl/src/ccd/taylor_matrix.cpp b/trunk/fcl/src/ccd/taylor_matrix.cpp
index 2018d3a8aa03c67b079cde2ab257b33bd61e9647..78944819017950ddac807b15af08ab126327e717 100644
--- a/trunk/fcl/src/ccd/taylor_matrix.cpp
+++ b/trunk/fcl/src/ccd/taylor_matrix.cpp
@@ -263,5 +263,52 @@ void TMatrix3::setTimeInterval(const boost::shared_ptr<TimeInterval>& time_inter
   v_[2].setTimeInterval(time_interval);
 }
 
+TMatrix3& TMatrix3::rotationConstrain()
+{
+  for(std::size_t i = 0; i < 3; ++i)
+  {
+    for(std::size_t j = 0; j < 3; ++j)
+    {
+      if(v_[i][j].r_[0] < -1) v_[i][j].r_[0] = -1;
+      else if(v_[i][j].r_[0] > 1) v_[i][j].r_[0] = 1;
+
+      if(v_[i][j].r_[1] < -1) v_[i][j].r_[1] = -1;
+      else if(v_[i][j].r_[1] > 1) v_[i][j].r_[1] = 1;
+
+      if((v_[i][j].r_[0] == -1) && (v_[i][j].r_[1] == 1))
+      {
+        v_[i][j].coeffs_[0] = v_[i][j].coeffs_[1] = v_[i][j].coeffs_[2] = v_[i][j].coeffs_[3] = 0;
+      }
+    }
+  }
+
+  return *this;
+}
+
+
+TMatrix3 rotationConstrain(const TMatrix3& m)
+{
+  TMatrix3 res;
+
+  for(std::size_t i = 0; i < 3; ++i)
+  {
+    for(std::size_t j = 0; j < 3; ++j)
+    {
+      if(m(i, j).r_[0] < -1) res(i, j).r_[0] = -1;
+      else if(m(i, j).r_[0] > 1) res(i, j).r_[0] = 1;
+
+      if(m(i, j).r_[1] < -1) res(i, j).r_[1] = -1;
+      else if(m(i, j).r_[1] > 1) res(i, j).r_[1] = 1;
+
+      if((m(i, j).r_[0] == -1) && (m(i, j).r_[1] == 1))
+      {
+        res(i, j).coeffs_[0] = res(i, j).coeffs_[1] = res(i, j).coeffs_[2] = res(i, j).coeffs_[3] = 0;
+      }
+    }
+  }
+
+  return res;
+}
+
 
 }