Revision: 23359
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23359&view=rev
Author: isucan
Date: 2009-08-31 02:58:00 +0000 (Mon, 31 Aug 2009)
Log Message:
-----------
began work on state handling
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h
pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp
pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state.h
pkg/trunk/motion_planning/planning_models/src/kinematic_state.cpp
Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-08-31 00:32:46 UTC (rev 23358)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-08-31 02:58:00 UTC (rev 23359)
@@ -11,7 +11,8 @@
rospack_link_boost(planning_models thread)
-rospack_add_library(planning_models2 src/kinematic_model.cpp)
+rospack_add_library(planning_models2 src/kinematic_model.cpp
+ src/kinematic_state.cpp)
rospack_link_boost(planning_models2 thread)
# Unit tests
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h 2009-08-31 00:32:46 UTC (rev 23358)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h 2009-08-31 02:58:00 UTC (rev 23359)
@@ -224,7 +224,7 @@
btTransform globalTrans;
/** \brief Recompute globalTrans and globalTransFwd */
- inline void computeTransform(void);
+ void computeTransform(void);
};
@@ -256,7 +256,7 @@
std::vector<std::string> touchLinks;
/** \brief Recompute globalTrans */
- inline void computeTransform(void);
+ void computeTransform(void);
};
@@ -327,6 +327,9 @@
/** \brief Get a group by its name */
const JointGroup* getGroup(const std::string &name) const;
+
+ /** \brief Get a group by its name */
+ JointGroup* getGroup(const std::string &name);
/** \brief Check if a group exists */
bool hasGroup(const std::string &name) const;
@@ -340,6 +343,9 @@
/** \brief Get a link by its name */
const Link* getLink(const std::string &link) const;
+ /** \brief Get a link by its name */
+ Link* getLink(const std::string &link);
+
/** \brief Check if a link exists */
bool hasLink(const std::string &name) const;
@@ -352,6 +358,9 @@
/** \brief Get a joint by its name */
const Joint* getJoint(const std::string &joint) const;
+ /** \brief Get a joint by its name */
+ Joint* getJoint(const std::string &joint);
+
/** \brief Check if a joint exists */
bool hasJoint(const std::string &name) const;
Added: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state.h (rev 0)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_state.h 2009-08-31 02:58:00 UTC (rev 23359)
@@ -0,0 +1,221 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, 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 the Willow Garage 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 Ioan Sucan */
+
+#ifndef PLANNING_MODELS_KINEMATIC_STATE_
+#define PLANNING_MODELS_KINEMATIC_STATE_
+
+#include "planning_models/kinematic_model.h"
+
+/** \brief Main namespace */
+namespace planning_models
+{
+
+ /** \brief A class that can hold the named parameters of this planning model */
+ class KinematicState
+ {
+ public:
+ KinematicState(const KinematicModel *model);
+ KinematicState(const KinematicState &sp);
+
+ ~KinematicState(void);
+
+ KinematicState &operator=(const KinematicState &rhs);
+
+ bool operator==(const KinematicState &rhs) const;
+
+ /** \brief Construct a default state: each value at 0.0, if
+ within bounds. Otherwise, select middle point. */
+ void defaultState(void);
+
+ /** \brief Construct a random state (within bounds) */
+ void randomState(void);
+
+ /** \brief Construct a random state for a group */
+ void randomStateGroup(const std::string &group);
+
+ /** \brief Construct a random state for a group */
+ void randomStateGroup(const KinematicModel::JointGroup *group);
+
+ /** \brief Perturb state. Each dimension is perturbed by a factor of its range */
+ void perturbState(double factor);
+
+ /** \brief Perturb state of a group. Each dimension is perturbed by a factor of its range */
+ void perturbStateGroup(double factor, const std::string &group);
+
+ /** \brief Perturb state of a group. Each dimension is perturbed by a factor of its range */
+ void perturbStateGroup(double factor, const KinematicModel::JointGroup *group);
+
+ /** \brief Update parameters so that they are within the specified bounds */
+ void enforceBounds(void);
+
+ /** \brief Update parameters so that they are within the specified bounds (for a specific group) */
+ void enforceBoundsGroup(const std::string &group);
+
+ /** \brief Update parameters so that they are within the specified bounds (for a specific group) */
+ void enforceBoundsGroup(const KinematicModel::JointGroup *group);
+
+ /** \brief Mark all values as unseen */
+ void reset(void);
+
+ /** \brief Mark all values in a group as unseen */
+ void resetGroup(const KinematicModel::JointGroup *group);
+
+ /** \brief Mark all values in a group as unseen */
+ void resetGroup(const std::string &group);
+
+ /** \brief Set all the parameters to a given value */
+ void setAll(const double value);
+
+ /** \brief Set all the parameters from a group to a given value */
+ void setAllInGroup(const double value, const std::string &group);
+
+ /** \brief Set all the parameters from a group to a given value */
+ void setAllInGroup(const double value, const KinematicModel::JointGroup *group);
+
+ /** \brief Set the parameters for the complete robot. */
+ bool setParams(const std::vector<double> ¶ms);
+
+ /** \brief Set the parameters for the complete robot. */
+ bool setParams(const double *params);
+
+ /** \brief Set the parameters for a given group. Return true if
+ any change was observed in either of the set
+ values. */
+ bool setParamsGroup(const std::vector<double> ¶ms, const std::string &group);
+
+ /** \brief Set the parameters for a given group. Return true if
+ any change was observed in either of the set
+ values. */
+ bool setParamsGroup(const std::vector<double> ¶ms, const KinematicModel::JointGroup *group);
+
+ /** \brief Set the parameters for a given group. Return true if
+ any change was observed in either of the set
+ values. */
+ bool setParamsGroup(const double *params, const std::string &group);
+
+ /** \brief Set the parameters for a given group. Return true if
+ any change was observed in either of the set
+ values. */
+ bool setParamsGroup(const double *params, const KinematicModel::JointGroup *group);
+
+ /** \brief Given the name of a joint, set the values of the
+ parameters describing the joint. Return true if any
+ change was observed in the set value */
+ bool setParamsJoint(const double *params, const std::string &name);
+
+ /** \brief Given the name of a joint, set the values of the
+ parameters describing the joint. Return true if any
+ change was observed in the set value */
+ bool setParamsJoint(const std::vector<double> ¶ms, const std::string &name);
+
+ /** \brief Given the names of some joints, set the values of the
+ parameters describing the joints. Return true if any
+ change was observed in the set values */
+ bool setParamsJoints(const double *params, const std::vector<std::string> &names);
+
+ /** \brief Given the names of some joints, set the values of the
+ parameters describing the joints. Return true if any
+ change was observed in the set values */
+ bool setParamsJoints(const std::vector<double> ¶ms, const std::vector<std::string> &names);
+
+ /** \brief Given the name of a joint, get the values of the
+ parameters describing the joint. */
+ const double* getParamsJoint(const std::string &name) const;
+
+ /** \brief Return the current value of the params */
+ const double* getParams(void) const;
+
+ /** \brief Copy the parameters for a given group to a destination address */
+ void copyParamsGroup(std::vector<double> ¶ms, const std::string &group) const;
+
+ /** \brief Copy the parameters for a given group to a destination address */
+ void copyParamsGroup(std::vector<double> ¶ms, const KinematicModel::JointGroup *group) const;
+
+ /** \brief Copy the parameters for a given group to a destination address */
+ void copyParamsGroup(double *params, const std::string &group) const;
+
+ /** \brief Copy the parameters for a given group to a destination address */
+ void copyParamsGroup(double *params, const KinematicModel::JointGroup *group) const;
+
+ /** \brief Copy all parameters to a destination address */
+ void copyParams(double *params) const;
+
+ /** \brief Copy all parameters to a destination address */
+ void copyParams(std::vector<double> ¶ms) const;
+
+ /** \brief Copy the parameters describing a given joint */
+ void copyParamsJoint(double *params, const std::string &name) const;
+
+ /** \brief Copy the parameters describing a given joint */
+ void copyParamsJoint(std::vector<double> ¶ms, const std::string &name) const;
+
+ /** \brief Copy the parameters describing a given set of joints */
+ void copyParamsJoints(double *params, const std::vector<std::string> &names) const;
+
+ /** \brief Copy the parameters describing a given set of joints */
+ void copyParamsJoints(std::vector<double> ¶ms, const std::vector<std::string> &names) const;
+
+ /** \brief Check if all params for a joint were seen */
+ bool seenJoint(const std::string &name) const;
+
+ /** \brief Check if all params in a group were seen */
+ bool seenAllGroup(const std::string &group) const;
+
+ /** \brief Check if all params in a group were seen */
+ bool seenAllGroup(const KinematicModel::JointGroup *group) const;
+
+ /** \brief Check if all params were seen */
+ bool seenAll(void) const;
+
+ /** \brief Print the data from the state to screen */
+ void print(std::ostream &out = std::cout) const;
+
+ /** \brief Print the missing joint index values */
+ void missing(std::ostream &out = std::cout);
+
+ protected:
+
+ /** \brief Copy data from another instance of this class */
+ void copyFrom(const KinematicState &sp);
+
+ const KinematicModel *owner_;
+ double *params_;
+ std::vector<bool> seen_;
+ };
+
+}
+
+#endif
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp 2009-08-31 00:32:46 UTC (rev 23358)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp 2009-08-31 02:58:00 UTC (rev 23359)
@@ -91,7 +91,7 @@
return modelName_;
}
-unsigned int planning_models::KinematicModel::getDimension(void) const
+inline unsigned int planning_models::KinematicModel::getDimension(void) const
{
return dimension_;
}
@@ -101,12 +101,12 @@
return stateBounds_;
}
-const btTransform& planning_models::KinematicModel::getRootTransform(void) const
+inline const btTransform& planning_models::KinematicModel::getRootTransform(void) const
{
return rootTransform_;
}
-void planning_models::KinematicModel::setRootTransform(const btTransform &transform)
+inline void planning_models::KinematicModel::setRootTransform(const btTransform &transform)
{
rootTransform_ = transform;
}
@@ -121,12 +121,12 @@
return planarJoints_;
}
-void planning_models::KinematicModel::lock(void)
+inline void planning_models::KinematicModel::lock(void)
{
lock_.lock();
}
-void planning_models::KinematicModel::unlock(void)
+inline void planning_models::KinematicModel::unlock(void)
{
lock_.unlock();
}
@@ -176,8 +176,6 @@
planning_models::KinematicModel::Joint* planning_models::KinematicModel::buildRecursive(Link *parent, const urdf::Link *link)
{
- ROS_INFO("%s", link->name.c_str());
-
Joint *joint = constructJoint(link->parent_joint.get(), stateBounds_);
joint->stateIndex = dimension_;
jointMap_[joint->name] = joint;
@@ -196,7 +194,7 @@
if (dynamic_cast<PlanarJoint*>(joint))
planarJoints_.push_back(joint->name);
- for (unsigned int i = 0 ; link->child_links.size() ; ++i)
+ for (unsigned int i = 0 ; i < link->child_links.size() ; ++i)
joint->after->after.push_back(buildRecursive(joint->after, link->child_links[i].get()));
return joint;
@@ -420,6 +418,18 @@
return it->second;
}
+planning_models::KinematicModel::Joint* planning_models::KinematicModel::getJoint(const std::string &name)
+{
+ std::map<std::string, Joint*>::iterator it = jointMap_.find(name);
+ if (it == jointMap_.end())
+ {
+ ROS_ERROR("Joint '%s' not found", name.c_str());
+ return NULL;
+ }
+ else
+ return it->second;
+}
+
const planning_models::KinematicModel::Link* planning_models::KinematicModel::getLink(const std::string &name) const
{
std::map<std::string, Link*>::const_iterator it = linkMap_.find(name);
@@ -432,6 +442,18 @@
return it->second;
}
+planning_models::KinematicModel::Link* planning_models::KinematicModel::getLink(const std::string &name)
+{
+ std::map<std::string, Link*>::iterator it = linkMap_.find(name);
+ if (it == linkMap_.end())
+ {
+ ROS_ERROR("Link '%s' not found", name.c_str());
+ return NULL;
+ }
+ else
+ return it->second;
+}
+
const planning_models::KinematicModel::JointGroup* planning_models::KinematicModel::getGroup(const std::string &name) const
{
std::map<std::string, JointGroup*>::const_iterator it = groupMap_.find(name);
@@ -444,6 +466,18 @@
return it->second;
}
+planning_models::KinematicModel::JointGroup* planning_models::KinematicModel::getGroup(const std::string &name)
+{
+ std::map<std::string, JointGroup*>::iterator it = groupMap_.find(name);
+ if (it == groupMap_.end())
+ {
+ ROS_ERROR("Joint group '%s' not found", name.c_str());
+ return NULL;
+ }
+ else
+ return it->second;
+}
+
void planning_models::KinematicModel::getGroups(std::vector<const JointGroup*> &groups) const
{
groups.clear();
@@ -554,7 +588,7 @@
getJoints(joints);
for (unsigned int i = 0 ; i < joints.size() ; ++i)
{
- printTransform(joints[i]->name, joints[i]->varTrans);
+ printTransform(joints[i]->name, joints[i]->varTrans, out);
out << std::endl;
}
out << "Link poses:" << std::endl;
@@ -562,7 +596,7 @@
getLinks(links);
for (unsigned int i = 0 ; i < links.size() ; ++i)
{
- printTransform(links[i]->name, links[i]->globalTrans);
+ printTransform(links[i]->name, links[i]->globalTrans, out);
out << std::endl;
}
}
@@ -629,7 +663,7 @@
}
-void planning_models::KinematicModel::Link::computeTransform(void)
+inline void planning_models::KinematicModel::Link::computeTransform(void)
{
globalTransFwd.mult(before->before ? before->before->globalTransFwd : owner->getRootTransform(), constTrans);
globalTransFwd *= before->varTrans;
@@ -652,7 +686,7 @@
delete shape;
}
-void planning_models::KinematicModel::AttachedBody::computeTransform(void)
+inline void planning_models::KinematicModel::AttachedBody::computeTransform(void)
{
globalTrans = owner->globalTrans * attachTrans;
}
Added: pkg/trunk/motion_planning/planning_models/src/kinematic_state.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_state.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_state.cpp 2009-08-31 02:58:00 UTC (rev 23359)
@@ -0,0 +1,468 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, 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 the Willow Garage 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 Ioan Sucan */
+
+#include <planning_models/kinematic_state.h>
+#include <ros/console.h>
+#include <algorithm>
+#include <sstream>
+#include <cmath>
+#include <cstdlib>
+
+planning_models::KinematicState::KinematicState(const KinematicModel *model) : owner_(model)
+{
+ params_ = model->getDimension() > 0 ? new double[model->getDimension()] : NULL;
+ seen_.resize(model->getDimension(), false);
+ defaultState();
+ reset();
+}
+
+planning_models::KinematicState::KinematicState(const KinematicState &sp) : owner_(sp.owner_), params_(NULL)
+{
+ copyFrom(sp);
+}
+
+planning_models::KinematicState::~KinematicState(void)
+{
+ if (params_)
+ delete[] params_;
+}
+
+planning_models::KinematicState& planning_models::KinematicState::operator=(const KinematicState &rhs)
+{
+ if (this != &rhs)
+ copyFrom(rhs);
+ return *this;
+}
+
+bool planning_models::KinematicState::operator==(const KinematicState &rhs) const
+{
+ const unsigned int dim = owner_->getDimension();
+ if (dim != rhs.owner_->getDimension())
+ return false;
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ if (fabs(params_[i] - rhs.params_[i]) > DBL_MIN)
+ return false;
+ return true;
+}
+
+void planning_models::KinematicState::copyFrom(const KinematicState &sp)
+{
+ owner_ = sp.owner_;
+ if (params_)
+ delete[] params_;
+ const unsigned int dim = owner_->getDimension();
+ params_ = dim > 0 ? new double[dim] : NULL;
+ if (params_)
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ params_[i] = sp.params_[i];
+ seen_ = sp.seen_;
+}
+
+void planning_models::KinematicState::defaultState(void)
+{
+ const unsigned int dim = owner_->getDimension();
+ const std::vector<double> &bounds = owner_->getStateBounds();
+
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ {
+ const unsigned int i2 = i << 1;
+ if (bounds[i2] <= 0.0 && bounds[i2 + 1] >= 0.0)
+ params_[i] = 0.0;
+ else
+ params_[i] = (bounds[i2] + bounds[i2 + 1]) / 2.0;
+ seen_[i] = true;
+ }
+}
+
+void planning_models::KinematicState::randomStateGroup(const std::string &group)
+{
+ randomStateGroup(owner_->getGroup(group));
+}
+
+void planning_models::KinematicState::randomStateGroup(const KinematicModel::JointGroup *group)
+{
+ const std::vector<double> &bounds = owner_->getStateBounds();
+ for (unsigned int i = 0 ; i < group->dimension ; ++i)
+ {
+ const unsigned int j = group->stateIndex[i];
+ const unsigned int j2 = j << 1;
+ params_[j] = (bounds[j2 + 1] - bounds[j2]) * ((double)rand() / (RAND_MAX + 1.0)) + bounds[j2];
+ seen_[j] = true;
+ }
+}
+
+void planning_models::KinematicState::randomState(void)
+{
+ const std::vector<double> &bounds = owner_->getStateBounds();
+ const unsigned int dim = owner_->getDimension();
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ {
+ const unsigned int i2 = i << 1;
+ params_[i] = (bounds[i2 + 1] - bounds[i2]) * ((double)rand() / (RAND_MAX + 1.0)) + bounds[i2];
+ seen_[i] = true;
+ }
+}
+
+void planning_models::KinematicState::perturbStateGroup(double factor, const std::string &group)
+{
+ perturbStateGroup(factor, owner_->getGroup(group));
+}
+
+void planning_models::KinematicState::perturbStateGroup(double factor, const KinematicModel::JointGroup *group)
+{
+ const std::vector<double> &bounds = owner_->getStateBounds();
+ for (unsigned int i = 0 ; i < group->dimension ; ++i)
+ {
+ const unsigned int j = group->stateIndex[i];
+ const unsigned int j2 = j << 1;
+ params_[j] += factor * (bounds[j2 + 1] - bounds[j2]) * (2.0 * ((double)rand() / (RAND_MAX + 1.0)) - 1.0);
+ }
+ enforceBoundsGroup(group);
+}
+
+void planning_models::KinematicState::perturbState(double factor)
+{
+ const std::vector<double> &bounds = owner_->getStateBounds();
+ const unsigned int dim = owner_->getDimension();
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ {
+ const unsigned int i2 = i << 1;
+ params_[i] += factor * (bounds[i2 + 1] - bounds[i2]) * (2.0 * ((double)rand() / (RAND_MAX + 1.0)) - 1.0);
+ }
+ enforceBounds();
+}
+
+void planning_models::KinematicState::enforceBoundsGroup(const std::string &group)
+{
+ enforceBoundsGroup(owner_->getGroup(group));
+}
+
+void planning_models::KinematicState::enforceBoundsGroup(const KinematicModel::JointGroup *group)
+{
+ const std::vector<double> &bounds = owner_->getStateBounds();
+ for (unsigned int i = 0 ; i < group->dimension ; ++i)
+ {
+ const unsigned int j = group->stateIndex[i];
+ const unsigned int j2 = j << 1;
+ if (params_[j] > bounds[j2 + 1])
+ params_[j] = bounds[j2 + 1];
+ else
+ if (params_[j] < bounds[j2])
+ params_[j] = bounds[j2];
+ }
+}
+
+void planning_models::KinematicState::enforceBounds(void)
+{
+ const std::vector<double> &bounds = owner_->getStateBounds();
+ const unsigned int dim = owner_->getDimension();
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ {
+ const unsigned int i2 = i << 1;
+ if (params_[i] > bounds[i2 + 1])
+ params_[i] = bounds[i2 + 1];
+ else
+ if (params_[i] < bounds[i2])
+ params_[i] = bounds[i2];
+ }
+}
+
+void planning_models::KinematicState::reset(void)
+{
+ const unsigned int dim = owner_->getDimension();
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ seen_[i] = false;
+}
+
+void planning_models::KinematicState::resetGroup(const std::string &group)
+{
+ resetGroup(owner_->getGroup(group));
+}
+
+void planning_models::KinematicState::resetGroup(const KinematicModel::JointGroup *group)
+{
+ for (unsigned int i = 0 ; i < group->dimension ; ++i)
+ seen_[group->stateIndex[i]] = false;
+}
+
+bool planning_models::KinematicState::seenAll(void) const
+{
+ const unsigned int dim = owner_->getDimension();
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ if (!seen_[i])
+ return false;
+ return true;
+}
+
+bool planning_models::KinematicState::seenAllGroup(const std::string &group) const
+{
+ return seenAllGroup(owner_->getGroup(group));
+}
+
+bool planning_models::KinematicState::seenAllGroup(const KinematicModel::JointGroup *group) const
+{
+ for (unsigned int i = 0 ; i < group->dimension ; ++i)
+ if (!seen_[group->stateIndex[i]])
+ return false;
+ return true;
+}
+
+bool planning_models::KinematicState::seenJoint(const std::string &name) const
+{
+ const KinematicModel::Joint *joint = owner_->getJoint(name);
+ for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
+ if (!seen_[joint->stateIndex + i])
+ return false;
+ return true;
+}
+
+void planning_models::KinematicState::missing(std::ostream &out)
+{
+ const unsigned int dim = owner_->getDimension();
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ if (!seen_[i])
+ out << i << " ";
+}
+
+const double* planning_models::KinematicState::getParamsJoint(const std::string &name) const
+{
+ const KinematicModel::Joint *joint = owner_->getJoint(name);
+ return params_ + joint->stateIndex;
+}
+
+bool planning_models::KinematicState::setParamsJoints(const double *params, const std::vector<std::string> &names)
+{
+ bool change = false;
+ int u = 0;
+ for (unsigned int i = 0 ; i < names.size() ; ++i)
+ {
+ const KinematicModel::Joint *joint = owner_->getJoint(names[i]);
+ bool ch = setParamsJoint(params + u, names[i]);
+ u += joint->usedParams;
+ change = change || ch;
+ }
+
+ return change;
+}
+
+bool planning_models::KinematicState::setParamsJoints(const std::vector<double> ¶ms, const std::vector<std::string> &names)
+{
+ return setParamsJoints(¶ms[0], names);
+}
+
+bool planning_models::KinematicState::setParamsJoint(const std::vector<double> ¶ms, const std::string &name)
+{
+ return setParamsJoint(¶ms[0], name);
+}
+
+bool planning_models::KinematicState::setParamsJoint(const double *params, const std::string &name)
+{
+ bool result = false;
+ const KinematicModel::Joint *joint = owner_->getJoint(name);
+
+ for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
+ {
+ unsigned int pos_i = joint->stateIndex + i;
+ if (params_[pos_i] != params[i] || !seen_[pos_i])
+ {
+ params_[pos_i] = params[i];
+ seen_[pos_i] = true;
+ result = true;
+ }
+ }
+
+ return result;
+}
+
+bool planning_models::KinematicState::setParams(const std::vector<double> ¶ms)
+{
+ return setParams(¶ms[0]);
+}
+
+bool planning_models::KinematicState::setParams(const double *params)
+{
+ const unsigned int dim = owner_->getDimension();
+ bool result = false;
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ if (params_[i] != params[i] || !seen_[i])
+ {
+ params_[i] = params[i];
+ seen_[i] = true;
+ result = true;
+ }
+ return result;
+}
+
+bool planning_models::KinematicState::setParamsGroup(const std::vector<double> ¶ms, const std::string &group)
+{
+ return setParamsGroup(¶ms[0], owner_->getGroup(group));
+}
+
+bool planning_models::KinematicState::setParamsGroup(const std::vector<double> ¶ms, const KinematicModel::JointGroup *group)
+{
+ return setParamsGroup(¶ms[0], group);
+}
+
+bool planning_models::KinematicState::setParamsGroup(const double *params, const std::string &group)
+{
+ return setParamsGroup(params, owner_->getGroup(group));
+}
+
+bool planning_models::KinematicState::setParamsGroup(const double *params, const KinematicModel::JointGroup *group)
+{
+ bool result = false;
+ for (unsigned int i = 0 ; i < group->dimension ; ++i)
+ {
+ unsigned int j = group->stateIndex[i];
+ if (params_[j] != params[i] || !seen_[j])
+ {
+ params_[j] = params[i];
+ seen_[j] = true;
+ result = true;
+ }
+ }
+ return result;
+}
+
+void planning_models::KinematicState::setAllInGroup(const double value, const std::string &group)
+{
+ setAllInGroup(value, owner_->getGroup(group));
+}
+
+void planning_models::KinematicState::setAllInGroup(const double value, const KinematicModel::JointGroup *group)
+{
+ for (unsigned int i = 0 ; i < group->dimension ; ++i)
+ {
+ unsigned int j = group->stateIndex[i];
+ params_[j] = value;
+ seen_[j] = true;
+ }
+}
+
+void planning_models::KinematicState::setAll(const double value)
+{
+ const unsigned int dim = owner_->getDimension();
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ {
+ params_[i] = value;
+ seen_[i] = true;
+ }
+}
+
+const double* planning_models::KinematicState::getParams(void) const
+{
+ return params_;
+}
+
+void planning_models::KinematicState::copyParamsJoint(double *params, const std::string &name) const
+{
+ const KinematicModel::Joint *joint = owner_->getJoint(name);
+ std::copy(params_ + joint->stateIndex, params_ + joint->stateIndex + joint->usedParams, params);
+}
+
+void planning_models::KinematicState::copyParamsJoint(std::vector<double> ¶ms, const std::string &name) const
+{
+ const KinematicModel::Joint *joint = owner_->getJoint(name);
+ params.resize(joint->usedParams);
+ std::copy(params_ + joint->stateIndex, params_ + joint->stateIndex + joint->usedParams, params.begin());
+}
+
+void planning_models::KinematicState::copyParams(double *params) const
+{
+ std::copy(params_, params_ + owner_->getDimension(), params);
+}
+
+void planning_models::KinematicState::copyParams(std::vector<double> ¶ms) const
+{
+ params.resize(owner_->getDimension());
+ std::copy(params_, params_ + owner_->getDimension(), params.begin());
+}
+
+void planning_models::KinematicState::copyParamsJoints(std::vector<double> ¶ms, const std::vector<std::string> &names) const
+{
+ params.clear();
+ for (unsigned int j = 0 ; j < names.size() ; ++j)
+ {
+ std::vector<double> p;
+ copyParamsJoint(p, names[j]);
+ params.insert(params.end(), p.begin(), p.end());
+ }
+}
+
+void planning_models::KinematicState::copyParamsGroup(double *params, const std::string &group) const
+{
+ copyParamsGroup(params, owner_->getGroup(group));
+}
+
+void planning_models::KinematicState::copyParamsGroup(std::vector<double> ¶ms, const std::string &group) const
+{
+ const KinematicModel::JointGroup *g = owner_->getGroup(group);
+ params.resize(g->dimension);
+ copyParamsGroup(¶ms[0], g);
+}
+
+void planning_models::KinematicState::copyParamsGroup(std::vector<double> ¶ms, const KinematicModel::JointGroup *group) const
+{
+ params.resize(group->dimension);
+ copyParamsGroup(¶ms[0], group);
+}
+
+void planning_models::KinematicState::copyParamsGroup(double *params, const KinematicModel::JointGroup *group) const
+{
+ for (unsigned int i = 0 ; i < group->dimension ; ++i)
+ params[i] = params_[group->stateIndex[i]];
+}
+
+void planning_models::KinematicState::print(std::ostream &out) const
+{
+ out << std::endl;
+ std::vector<const KinematicModel::Joint*> joints;
+ owner_->getJoints(joints);
+
+ for (unsigned int i = 0 ; i < joints.size() ; ++i)
+ {
+ out << joints[i]->name;
+ if (!seenJoint(joints[i]->name))
+ out << "[ *** UNSEEN *** ]";
+ out << ": ";
+ for (unsigned int j = 0 ; j < joints[i]->usedParams ; ++j)
+ out << params_[joints[i]->stateIndex + j] << std::endl;
+ }
+ out << std::endl;
+ for (unsigned int i = 0; i < owner_->getDimension() ; ++i)
+ out << params_[i] << " ";
+ out << std::endl;
+}
Modified: pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp 2009-08-31 00:32:46 UTC (rev 23358)
+++ pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp 2009-08-31 02:58:00 UTC (rev 23359)
@@ -182,6 +182,232 @@
delete model;
}
+
+TEST(FK, OneRobot)
+{
+ static const std::string MODEL2 =
+ "<?xml version=\"1.0\" ?>"
+ "<robot name=\"one_robot\">"
+ "<joint name=\"base_joint\" type=\"planar\">"
+ " <parent link=\"world\"/>"
+ " <child link=\"base_link\"/>"
+ " <origin rpy=\" 0.0 -0.2 0 \" xyz=\"1 0 0 \"/>"
+ "</joint>"
+ "<link name=\"base_link\">"
+ " <inertial>"
+ " <mass value=\"2.81\"/>"
+ " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
+ " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
+ " </inertial>"
+ " <collision name=\"my_collision\">"
+ " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
+ " <geometry>"
+ " <box size=\"1 2 1\" />"
+ " </geometry>"
+ " </collision>"
+ " <visual>"
+ " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
+ " <geometry>"
+ " <box size=\"1 2 1\" />"
+ " </geometry>"
+ " </visual>"
+ "</link>"
+ "<joint name=\"joint_a\" type=\"continuous\">"
+ " <axis xyz=\"0 0 1\"/>"
+ " <parent link=\"base_link\"/>"
+ " <child link=\"link_a\"/>"
+ " <origin rpy=\" 0.0 0 0 \" xyz=\"0.0 0 0 \"/>"
+ "</joint>"
+ "<link name=\"link_a\">"
+ " <inertial>"
+ " <mass value=\"1.0\"/>"
+ " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
+ " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
+ " </inertial>"
+ " <collision>"
+ " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
+ " <geometry>"
+ " <box size=\"1 2 1\" />"
+ " </geometry>"
+ " </collision>"
+ " <visual>"
+ " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
+ " <geometry>"
+ " <box size=\"1 2 1\" />"
+ " </geometry>"
+ " </visual>"
+ "</link>"
+ "<joint name=\"joint_b\" type=\"fixed\">"
+ " <parent link=\"link_a\"/>"
+ " <child link=\"link_b\"/>"
+ " <origin rpy=\" 0.0 -0.42 0 \" xyz=\"0.0 0.5 0 \"/>"
+ "</joint>"
+ "<link name=\"link_b\">"
+ " <inertial>"
+ " <mass value=\"1.0\"/>"
+ " <origin rpy=\"0 0 0\" xyz=\"0.0 0.0 .0\"/>"
+ " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
+ " </inertial>"
+ " <collision>"
+ " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
+ " <geometry>"
+ " <box size=\"1 2 1\" />"
+ " </geometry>"
+ " </collision>"
+ " <visual>"
+ " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
+ " <geometry>"
+ " <box size=\"1 2 1\" />"
+ " </geometry>"
+ " </visual>"
+ "</link>"
+ " <joint name=\"joint_c\" type=\"prismatic\">"
+ " <axis xyz=\"1 0 0\"/>"
+ " <limit effort=\"100.0\" lower=\"0.0\" upper=\"0.09\" velocity=\"0.2\"/>"
+ " <safety_controller k_position=\"20.0\" k_velocity=\"500.0\" soft_lower_limit=\"0.0\" soft_upper_limit=\"0.089\"/>"
+ " <parent link=\"link_b\"/>"
+ " <child link=\"link_c\"/>"
+ " <origin rpy=\" 0.0 0.42 0.0 \" xyz=\"0.0 -0.1 0 \"/>"
+ " </joint>"
+ "<link name=\"link_c\">"
+ " <inertial>"
+ " <mass value=\"1.0\"/>"
+ " <origin rpy=\"0 0 0\" xyz=\"0.0 0 .0\"/>"
+ " <inertia ixx=\"0.1\" ixy=\"-0.2\" ixz=\"0.5\" iyy=\"-.09\" iyz=\"1\" izz=\"0.101\"/>"
+ " </inertial>"
+ " <collision>"
+ " <origin rpy=\"0 0 0\" xyz=\"0 0 0\"/>"
+ " <geometry>"
+ " <box size=\"1 2 1\" />"
+ " </geometry>"
+ " </collision>"
+ " <visual>"
+ " <origin rpy=\"0 0 0\" xyz=\"0.0 0 0\"/>"
+ " <geometry>"
+ " <box size=\"1 2 1\" />"
+ " </geometry>"
+ " </visual>"
+ "</link>"
+ "</robot>";
+
+ static const std::string MODEL2_INFO =
+ "Number of robots = 1\n"
+ "Complete model state dimension = 5\n"
+ "State bounds: [0.00000, 0.00000] [0.00000, 0.00000] [-3.14159, 3.14159] [-3.14159, 3.14159] [0.01000, 0.38600] \n"
+ "Parameter index:\n"
+ "base_link_joint = 0\n"
+ "link_a_joint = 3\n"
+ "link_c_joint = 4\n"
+ "Parameter name:\n"
+ "0 = base_link_joint\n"
+ "1 = base_link_joint\n"
+ "2 = base_link_joint\n"
+ "3 = link_a_joint\n"
+ "4 = link_c_joint\n"
+ "Floating joints at: \n"
+ "Planar joints at: 0 base_link_joint \n"
+ "Available groups: base \n"
+ "Group base with ID 0 has 1 roots: base_link_joint \n"
+ "The state components for this group are: 0 1 2 3 4 base_link_joint base_link_joint base_link_joint link_a_joint link_c_joint \n";
+
+ urdf::Model urdfModel;
+ urdfModel.initString(MODEL2);
+
+ std::map < std::string, std::vector<std::string> > groups;
+ groups["base"].push_back("base_joint");
+ groups["base"].push_back("joint_a");
+ groups["base"].push_back("joint_b");
+ groups["base"].push_back("joint_c");
+
+ planning_models::KinematicModel *model = new planning_models::KinematicModel(urdfModel, groups);
+
+
+ EXPECT_EQ((unsigned int)5, model->getDimension());
+
+ double param[5] = { 1, 1, 0.5, -0.5, 0.1 };
+ model->getGroup("base")->computeTransforms(param);
+
+ std::stringstream ss1;
+ model->printModelInfo(ss1);
+ // EXPECT_TRUE(sameStringIgnoringWS(MODEL2_INFO, ss1.str()));
+ ROS_INFO("'%s'", ss1.str().c_str());
+
+ // make sure applying the state works for the entire robot
+ model->printTransforms(ss1);
+
+ model->computeTransforms(param);
+
+ std::stringstream ss2;
+ model->printModelInfo(ss2);
+ model->printTransforms(ss2);
+
+ // EXPECT_EQ(ss1.str(), ss2.str());
+
+ EXPECT_NEAR(1.0, model->getLink("base_link")->globalTrans.getOrigin().x(), 1e-5);
+ EXPECT_NEAR(1.0, model->getLink("base_link")->globalTrans.getOrigin().y(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("base_link")->globalTrans.getOrigin().z(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("base_link")->globalTrans.getRotation().x(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("base_link")->globalTrans.getRotation().y(), 1e-5);
+ EXPECT_NEAR(0.247404, model->getLink("base_link")->globalTrans.getRotation().z(), 1e-5);
+ EXPECT_NEAR(0.968912, model->getLink("base_link")->globalTrans.getRotation().w(), 1e-5);
+
+ EXPECT_NEAR(1.0, model->getLink("link_a")->globalTrans.getOrigin().x(), 1e-5);
+ EXPECT_NEAR(1.0, model->getLink("link_a")->globalTrans.getOrigin().y(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("link_a")->globalTrans.getOrigin().z(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("link_a")->globalTrans.getRotation().x(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("link_a")->globalTrans.getRotation().y(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("link_a")->globalTrans.getRotation().z(), 1e-5);
+ EXPECT_NEAR(1.0, model->getLink("link_a")->globalTrans.getRotation().w(), 1e-5);
+
+ EXPECT_NEAR(1.0, model->getLink("link_b")->globalTrans.getOrigin().x(), 1e-5);
+ EXPECT_NEAR(1.5, model->getLink("link_b")->globalTrans.getOrigin().y(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("link_b")->globalTrans.getOrigin().z(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("link_b")->globalTrans.getRotation().x(), 1e-5);
+ EXPECT_NEAR(-0.20846, model->getLink("link_b")->globalTrans.getRotation().y(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("link_b")->globalTrans.getRotation().z(), 1e-5);
+ EXPECT_NEAR(0.978031, model->getLink("link_b")->globalTrans.getRotation().w(), 1e-5);
+
+ EXPECT_NEAR(1.1, model->getLink("link_c")->globalTrans.getOrigin().x(), 1e-5);
+ EXPECT_NEAR(1.4, model->getLink("link_c")->globalTrans.getOrigin().y(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("link_c")->globalTrans.getOrigin().z(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("link_c")->globalTrans.getRotation().x(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("link_c")->globalTrans.getRotation().y(), 1e-5);
+ EXPECT_NEAR(0.0, model->getLink("link_c")->globalTrans.getRotation().z(), 1e-5);
+ EXPECT_NEAR(1.0, model->getLink("link_c")->globalTrans.getRotation().w(), 1e-5);
+
+ /*
+ planning_models::StateParams *sp = model->newStateParams();
+ EXPECT_FALSE(sp->seenAll());
+
+ double tmpParam[3];
+ tmpParam[0] = 0.1;
+ sp->setParamsJoint(tmpParam, "link_a_joint");
+ EXPECT_FALSE(sp->seenAll());
+ EXPECT_TRUE(sp->seenJoint("link_a_joint"));
+
+ tmpParam[0] = -1.0;
+ sp->setParamsJoint(tmpParam, "link_c_joint");
+ EXPECT_FALSE(sp->seenAll());
+
+ tmpParam[0] = 0.5; tmpParam[1] = 0.4; tmpParam[2] = 1.1;
+ sp->setParamsJoint(tmpParam, "base_link_joint");
+ EXPECT_TRUE(sp->seenAll());
+
+ EXPECT_EQ(0.5, sp->getParams()[0]);
+ EXPECT_EQ(0.4, sp->getParams()[1]);
+ EXPECT_EQ(1.1, sp->getParams()[2]);
+ EXPECT_EQ(0.1, sp->getParams()[3]);
+ EXPECT_EQ(-1.0, sp->getParams()[4]);
+
+ planning_models::StateParams sp_copy = *sp;
+ EXPECT_TRUE(sp_copy == *sp);
+
+ delete sp;
+ */
+ delete model;
+}
+
+
int main(int argc, char **argv)
{
testing::InitGoogleTest(&argc, argv);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|