Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[17061] pkg/trunk/motion_planning/ planning_environment
|
From: <is...@us...> - 2009-06-13 04:43:54
|
Revision: 17061
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17061&view=rev
Author: isucan
Date: 2009-06-13 04:43:52 +0000 (Sat, 13 Jun 2009)
Log Message:
-----------
checking of paths and states for validity; this does not depend on any type of planning
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h
pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
Modified: pkg/trunk/motion_planning/planning_environment/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-06-13 02:30:38 UTC (rev 17060)
+++ pkg/trunk/motion_planning/planning_environment/CMakeLists.txt 2009-06-13 04:43:52 UTC (rev 17061)
@@ -9,7 +9,8 @@
src/collision_models.cpp
src/kinematic_model_state_monitor.cpp
src/collision_space_monitor.cpp
- src/kinematic_state_constraint_evaluator.cpp)
+ src/kinematic_state_constraint_evaluator.cpp
+ src/planning_monitor.cpp)
#rospack_add_openmp_flags(planning_environment)
# Tests
Modified: pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h 2009-06-13 02:30:38 UTC (rev 17060)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/kinematic_state_constraint_evaluator.h 2009-06-13 04:43:52 UTC (rev 17061)
@@ -68,6 +68,9 @@
/** Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
virtual bool decide(const double *params, const int groupID) const = 0;
+ /** Decide whether the constraint is satisfied. The kinematic model is assumed to be at the state we want to decide. */
+ virtual bool decide(const double *params) const;
+
/** Print the constraint data */
virtual void print(std::ostream &out = std::cout) const
{
@@ -183,6 +186,9 @@
/** Decide whether the set of constraints is satisfied */
bool decide(const double *params, int groupID) const;
+ /** Decide whether the set of constraints is satisfied */
+ bool decide(const double *params) const;
+
/** Print the constraint data */
void print(std::ostream &out = std::cout) const;
Added: pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h
===================================================================
--- pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/include/planning_environment/planning_monitor.h 2009-06-13 04:43:52 UTC (rev 17061)
@@ -0,0 +1,94 @@
+/*********************************************************************
+* 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_ENVIRONMENT_PLANNING_MONITOR_
+#define PLANNING_ENVIRONMENT_PLANNING_MONITOR_
+
+#include "planning_environment/collision_space_monitor.h"
+#include <motion_planning_msgs/KinematicPath.h>
+#include <motion_planning_msgs/KinematicConstraints.h>
+
+namespace planning_environment
+{
+
+ /** @b PlanningMonitor is a class which in addition to being aware
+ of a robot model, and the collision model is also aware of
+ constraints and can check the validity of states and paths.
+ */
+
+ class PlanningMonitor : public CollisionSpaceMonitor
+ {
+ public:
+
+ PlanningMonitor(CollisionModels *cm, std::string frame_id) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm), frame_id)
+ {
+ }
+
+ PlanningMonitor(CollisionModels *cm) : CollisionSpaceMonitor(static_cast<CollisionModels*>(cm))
+ {
+ }
+
+ virtual ~PlanningMonitor(void)
+ {
+ }
+
+ /** Check if the full state of the robot is valid */
+ bool isStateValidOnPath(const planning_models::KinematicModel::StateParams *state) const;
+
+ /** Check if the full state of the robot is valid */
+ bool isStateValidAtGoal(const planning_models::KinematicModel::StateParams *state) const;
+
+ /** Check if the path is valid */
+ bool isPathValid(const motion_planning_msgs::KinematicPath &path);
+
+ /** Set the kinematic constraints the monitor should use when checking a path */
+ void setPathConstraints(const motion_planning_msgs::KinematicConstraints &kc);
+
+ /** Set the kinematic constraints the monitor should use when checking a path's last state (the goal) */
+ void setGoalConstraints(const motion_planning_msgs::KinematicConstraints &kc);
+
+ protected:
+
+ void bringConstraintsToModelFrame(motion_planning_msgs::KinematicConstraints &kc);
+
+ motion_planning_msgs::KinematicConstraints kcPath_;
+ motion_planning_msgs::KinematicConstraints kcGoal_;
+ };
+
+
+}
+
+#endif
Modified: pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp 2009-06-13 02:30:38 UTC (rev 17060)
+++ pkg/trunk/motion_planning/planning_environment/src/kinematic_state_constraint_evaluator.cpp 2009-06-13 04:43:52 UTC (rev 17061)
@@ -38,6 +38,11 @@
#include <tf/transform_datatypes.h>
#include <cassert>
+bool planning_environment::KinematicConstraintEvaluator::decide(const double *params) const
+{
+ return decide(params, -1);
+}
+
bool planning_environment::JointConstraintEvaluator::use(const planning_models::KinematicModel *kmodel, const ros::Message *kc)
{
const motion_planning_msgs::JointConstraint *jc = dynamic_cast<const motion_planning_msgs::JointConstraint*>(kc);
@@ -54,10 +59,10 @@
m_jc = jc;
return true;
}
-
+
bool planning_environment::JointConstraintEvaluator::decide(const double *params, const int groupID) const
{
- const double *val = params + m_kmodel->getJointIndexInGroup(m_joint->name, groupID);
+ const double *val = params + (groupID >= 0 ? m_kmodel->getJointIndexInGroup(m_joint->name, groupID) : m_kmodel->getJointIndex(m_joint->name));
assert(m_jc.value.size() == m_jc.toleranceBelow.size() && m_jc.value.size() == m_jc.toleranceAbove.size());
for (unsigned int i = 0 ; i < m_jc.value.size() ; ++i)
@@ -385,6 +390,11 @@
return true;
}
+bool planning_environment::KinematicConstraintEvaluatorSet::decide(const double *params) const
+{
+ return decide(params, -1);
+}
+
void planning_environment::KinematicConstraintEvaluatorSet::print(std::ostream &out) const
{
out << m_kce.size() << " kinematic constraints" << std::endl;
Added: pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_environment/src/planning_monitor.cpp 2009-06-13 04:43:52 UTC (rev 17061)
@@ -0,0 +1,177 @@
+/*********************************************************************
+* 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_environment/planning_monitor.h"
+#include "planning_environment/kinematic_state_constraint_evaluator.h"
+
+void planning_environment::PlanningMonitor::setPathConstraints(const motion_planning_msgs::KinematicConstraints &kc)
+{
+ kcPath_ = kc;
+ bringConstraintsToModelFrame(kcPath_);
+}
+
+void planning_environment::PlanningMonitor::setGoalConstraints(const motion_planning_msgs::KinematicConstraints &kc)
+{
+ kcGoal_ = kc;
+ bringConstraintsToModelFrame(kcPath_);
+}
+
+void planning_environment::PlanningMonitor::bringConstraintsToModelFrame(motion_planning_msgs::KinematicConstraints &kc)
+{
+ for (unsigned int i = 0; i < kc.pose.size() ; ++i)
+ tf_->transformPose(getFrameId(), kc.pose[i].pose, kc.pose[i].pose);
+}
+
+bool planning_environment::PlanningMonitor::isStateValidOnPath(const planning_models::KinematicModel::StateParams *state) const
+{
+ getEnvironmentModel()->lock();
+ getKinematicModel()->lock();
+
+ // figure out the poses of the robot model
+ getKinematicModel()->computeTransforms(state->getParams());
+ // update the collision space
+ getEnvironmentModel()->updateRobotModel();
+
+ // check for collision
+ bool valid = !getEnvironmentModel()->isCollision();
+
+ if (valid)
+ {
+ KinematicConstraintEvaluatorSet ks;
+ ks.add(getKinematicModel(), kcPath_.joint);
+ ks.add(getKinematicModel(), kcPath_.pose);
+ valid = ks.decide(state->getParams());
+ }
+
+ getKinematicModel()->unlock();
+ getEnvironmentModel()->unlock();
+
+ return valid;
+}
+
+bool planning_environment::PlanningMonitor::isStateValidAtGoal(const planning_models::KinematicModel::StateParams *state) const
+{
+ getEnvironmentModel()->lock();
+ getKinematicModel()->lock();
+
+ // figure out the poses of the robot model
+ getKinematicModel()->computeTransforms(state->getParams());
+ // update the collision space
+ getEnvironmentModel()->updateRobotModel();
+
+ // check for collision
+ bool valid = !getEnvironmentModel()->isCollision();
+
+ if (valid)
+ {
+ KinematicConstraintEvaluatorSet ks;
+ ks.add(getKinematicModel(), kcPath_.joint);
+ ks.add(getKinematicModel(), kcPath_.pose);
+ ks.add(getKinematicModel(), kcGoal_.joint);
+ ks.add(getKinematicModel(), kcGoal_.pose);
+ valid = ks.decide(state->getParams());
+ }
+
+ getKinematicModel()->unlock();
+ getEnvironmentModel()->unlock();
+
+ return valid;
+}
+
+bool planning_environment::PlanningMonitor::isPathValid(const motion_planning_msgs::KinematicPath &path)
+{
+ planning_models::KinematicModel::StateParams *sp = getKinematicModel()->newStateParams();
+ sp->setParams(path.start_state.vals);
+
+ KinematicConstraintEvaluatorSet ks;
+ ks.add(getKinematicModel(), kcPath_.joint);
+ ks.add(getKinematicModel(), kcPath_.pose);
+
+ getEnvironmentModel()->lock();
+ getKinematicModel()->lock();
+
+ // figure out the poses of the robot model
+ getKinematicModel()->computeTransforms(sp->getParams());
+ // update the collision space
+ getEnvironmentModel()->updateRobotModel();
+
+ bool valid = true;
+
+ // get the joints this path is for
+ std::vector<planning_models::KinematicModel::Joint*> joints(path.names.size());
+ for (unsigned int j = 0 ; j < joints.size() ; ++j)
+ joints[j] = getKinematicModel()->getJoint(path.names[j]);
+
+ // check every state
+ for (unsigned int i = 0 ; valid && i < path.states.size() ; ++i)
+ {
+ unsigned int u = 0;
+ for (unsigned int j = 0 ; j < joints.size() ; ++j)
+ {
+ /* copy the parameters that describe the joint */
+ std::vector<double> params;
+ for (unsigned int k = 0 ; k < joints[j]->usedParams ; ++k)
+ params.push_back(path.states[i].vals[u + k]);
+ u += joints[j]->usedParams;
+
+ /* set the parameters */
+ sp->setParamsJoint(params, joints[j]->name);
+ }
+ getKinematicModel()->computeTransforms(sp->getParams());
+ getEnvironmentModel()->updateRobotModel();
+
+ // check for collision
+ valid = !getEnvironmentModel()->isCollision();
+
+ // check for validity
+ if (valid)
+ valid = ks.decide(sp->getParams());
+ }
+
+ // if we got to the last state, we also check the goal constraints
+ if (valid)
+ {
+ ks.add(getKinematicModel(), kcGoal_.joint);
+ ks.add(getKinematicModel(), kcGoal_.pose);
+ valid = ks.decide(sp->getParams());
+ }
+
+ getKinematicModel()->unlock();
+ getEnvironmentModel()->unlock();
+
+ delete sp;
+ return valid;
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
