code for articulated model

git-svn-id: https://kforge.ros.org/fcl/fcl_ros@188 253336fb-580f-4252-a368-f3cef5a2a82b

trunk/fcl/src/CMakeLists.txt

@@ -42,6 +42,10 @@ add_library(${PROJECT_NAME} SHARED
   narrowphase/gjk_libccd.cpp
   narrowphase/narrowphase.cpp
   broadphase/hierarchy_tree.cpp
+  articulated_model/joint_config.cpp
+  articulated_model/joint.cpp
+  articulated_model/model_config.cpp
+  articulated_model/model.cpp
   profile.cpp
   collision_data.cpp)
 

trunk/fcl/src/articulated_model/joint.cpp

+/*
+ * 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/articulated_model/joint.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)
+{
+}
+
+std::string Joint::getName() const
+{
+  return name_;
+}
+
+std::size_t Joint::getDOFs() const
+{
+  return dofs_num_;
+}
+
+bool Joint::isCompatible(const JointConfig& q) const
+{
+  return this == q.getJoint().get();
+}
+
+PrismaticJoint::PrismaticJoint(const std::string& name, const Vec3f& axis) :
+  Joint(name, 1),
+  axis_(axis)
+{
+  axis_.normalize();
+}
+
+Vec3f PrismaticJoint::getAxis() const
+{
+  return axis_;
+}
+
+Transform3f PrismaticJoint::append(const JointConfig& q, const Transform3f& t) const
+{
+  BOOST_ASSERT(isCompatible(q));
+  /// axis_ is in local frame
+  return Transform3f(axis_ * q[0]) * t; 
+}
+
+RevoluteJoint::RevoluteJoint(const std::string& name, const Vec3f& axis) :
+  Joint(name, 1),
+  axis_(axis)
+{
+  axis_.normalize();
+}
+
+Vec3f RevoluteJoint::getAxis() const
+{
+  return axis_;
+}
+
+Transform3f RevoluteJoint::append(const JointConfig& q, const Transform3f& t) const
+{
+  BOOST_ASSERT(isCompatible(q));
+  Quaternion3f quat;
+  quat.fromAxisAngle(axis_, q[0]);
+  return Transform3f(quat) * t;
+}
+
+SphericJoint::SphericJoint(const std::string& name) :
+  Joint(name, 3)
+{}
+
+Transform3f SphericJoint::append(const JointConfig& q, const Transform3f& t) const
+{
+  BOOST_ASSERT(isCompatible(q));
+  Matrix3f rot;
+  rot.setEulerYPR(q[0], q[1], q[2]);
+  return Transform3f(rot) * t;
+}
+
+
+
+
+
+}

trunk/fcl/src/articulated_model/joint_config.cpp

+/*
+ * 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/articulated_model/joint_config.h"
+#include "fcl/articulated_model/joint.h"
+
+namespace fcl
+{
+
+JointConfig::JointConfig() {}
+
+JointConfig::JointConfig(const JointConfig& joint_cfg) :
+  joint_(joint_cfg.joint_),
+  values_(joint_cfg.values_)
+{
+}
+
+JointConfig::JointConfig(boost::shared_ptr<Joint> joint, FCL_REAL default_value) :
+  joint_(joint)
+{
+  values_.resize(joint->getDOFs(), default_value);
+}
+
+std::size_t JointConfig::size() const
+{
+  return values_.size();
+}
+
+bool JointConfig::operator == (const JointConfig& joint_cfg) const
+{
+  return (joint_ == joint_cfg.joint_) &&
+    std::equal(values_.begin(), values_.end(), joint_cfg.values_.begin());
+}
+
+boost::shared_ptr<Joint> JointConfig::getJoint() const
+{
+  return joint_;
+}
+
+}

trunk/fcl/src/articulated_model/model.cpp

+/*
+ * 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/articulated_model/model.h"
+#include "fcl/articulated_model/model_config.h"
+#include <boost/assert.hpp>
+
+namespace fcl
+{
+
+Model::Model(const std::string& name) :
+  name_(name)
+{
+}
+
+boost::shared_ptr<Model> Model::getSharedPtr()
+{
+  return shared_from_this();
+}
+
+std::string Model::getName() const
+{
+  return name_;
+}
+
+void Model::addJoint(boost::shared_ptr<Joint> joint, const std::string& parent_name)
+{
+  if(parent_name == "")
+    joints_tree_.createRoot(joint->getName(), joint);
+  else
+    joints_tree_.createNode(joint->getName(), joint, parent_name);
+}
+
+boost::shared_ptr<Joint> Model::getJointByName(const std::string& joint_name) const
+{
+  boost::shared_ptr<GeneralTreeNode<std::string, boost::shared_ptr<Joint> > > node = joints_tree_.getNodeByKey(joint_name);
+
+  return node->getValue();
+}
+
+boost::shared_ptr<Joint> Model::getJointParentByName(const std::string& joint_name) 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();
+
+  if(parent_node.use_count() == 0)
+    return boost::shared_ptr<Joint>();
+  else
+    return parent_node->getValue();
+}
+
+boost::shared_ptr<Joint> Model::getJointParent(boost::shared_ptr<Joint> joint) const
+{
+  return getJointParentByName(joint->getName());
+}
+
+bool Model::hasJointParentByName(const std::string& joint_name) const
+{
+  boost::shared_ptr<GeneralTreeNode<std::string, boost::shared_ptr<Joint> > > node = joints_tree_.getNodeByKey(joint_name);
+
+  return node->hasParent();
+}
+
+bool Model::hasJointParent(boost::shared_ptr<Joint> joint) const
+{
+  return hasJointParentByName(joint->getName());
+}
+
+std::size_t Model::getJointNum() const
+{
+  return joints_tree_.getNodesNum();
+}
+
+std::map<std::string, boost::shared_ptr<Joint> > Model::getJointsMap() const
+{
+  return joints_tree_.getValuesMap();
+}
+
+bool Model::isCompatible(const ModelConfig& model_config) const
+{
+  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;
+
+  for(it = joints_map.begin(); it != joints_map.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;
+  }
+
+  return true;
+}
+
+std::vector<boost::shared_ptr<Joint> > Model::getJointsChain(const std::string& last_joint_name) const
+{
+  std::vector<boost::shared_ptr<Joint> > chain;
+  boost::shared_ptr<Joint> joint = getJointByName(last_joint_name);
+
+  while(joint.use_count() != 0)
+  {
+    chain.push_back(joint);
+    joint = getJointParent(joint);
+  }
+
+  return chain;
+}
+
+}

trunk/fcl/src/articulated_model/model_config.cpp

+/*
+ * 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/articulated_model/model_config.h"
+#include "fcl/articulated_model/joint.h"
+#include <algorithm>
+
+namespace fcl
+{
+
+ModelConfig::ModelConfig() {}
+
+ModelConfig::ModelConfig(const ModelConfig& model_cfg) :
+  joint_cfgs_map_(model_cfg.joint_cfgs_map_)
+{}
+
+ModelConfig::ModelConfig(std::map<std::string, boost::shared_ptr<Joint> > joints_map)
+{
+  std::map<std::string, boost::shared_ptr<Joint> >::iterator it;
+  for(it = joints_map.begin(); it != joints_map.end(); ++it)
+    joint_cfgs_map_[it->first] = JointConfig(it->second);
+}
+
+JointConfig ModelConfig::getJointConfigByJointName(const std::string& joint_name) const
+{
+  std::map<std::string, JointConfig>::const_iterator it = joint_cfgs_map_.find(joint_name);
+  BOOST_ASSERT_MSG((it != joint_cfgs_map_.end()), "Joint name not valid");
+
+  return it->second;
+}
+
+JointConfig& ModelConfig::getJointConfigByJointName(const std::string& joint_name)
+{
+  std::map<std::string, JointConfig>::iterator it = joint_cfgs_map_.find(joint_name);
+  BOOST_ASSERT_MSG((it != joint_cfgs_map_.end()), "Joint name not valid");
+
+  return it->second;
+}
+
+JointConfig ModelConfig::getJointConfigByJoint(boost::shared_ptr<Joint> joint) const
+{
+  return getJointConfigByJointName(joint->getName());
+}
+
+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());
+}
+
+}