[Personalrobots-commit] SF.net SVN: personalrobots:[23359] pkg/trunk/motion_planning/ planning_models

[<is...@us...>]

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> &params);
+	
+	/** \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> &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 std::vector<double> &params, 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> &params, 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> &params, 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> &params, const std::string &group) const;
+	
+	/** \brief Copy the parameters for a given group to a destination address */
+	void copyParamsGroup(std::vector<double> &params, 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> &params) 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> &params, 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> &params, 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> &params, const std::vector<std::string> &names)
+{
+    return setParamsJoints(&params[0], names);
+}
+
+bool planning_models::KinematicState::setParamsJoint(const std::vector<double> &params, const std::string &name)
+{
+    return setParamsJoint(&params[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> &params)
+{ 
+    return setParams(&params[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> &params, const std::string &group)
+{
+    return setParamsGroup(&params[0], owner_->getGroup(group));
+}
+
+bool planning_models::KinematicState::setParamsGroup(const std::vector<double> &params, const KinematicModel::JointGroup *group)
+{
+    return setParamsGroup(&params[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> &params, 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> &params) const
+{
+    params.resize(owner_->getDimension());
+    std::copy(params_, params_ + owner_->getDimension(), params.begin());
+}
+
+void planning_models::KinematicState::copyParamsJoints(std::vector<double> &params, 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> &params, const std::string &group) const
+{
+    const KinematicModel::JointGroup *g = owner_->getGroup(group);
+    params.resize(g->dimension);
+    copyParamsGroup(&params[0], g);
+}
+
+void planning_models::KinematicState::copyParamsGroup(std::vector<double> &params, const KinematicModel::JointGroup *group) const
+{ 
+    params.resize(group->dimension);
+    copyParamsGroup(&params[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.