Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[4096] pkg/trunk
|
From: <is...@us...> - 2008-09-09 17:50:54
|
Revision: 4096
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4096&view=rev
Author: isucan
Date: 2008-09-09 17:51:02 +0000 (Tue, 09 Sep 2008)
Log Message:
-----------
first version of a motion_validator
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt
pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h
pkg/trunk/motion_planning/kinematic_planning/include/RKPPlannerSetup.h
pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
Added Paths:
-----------
pkg/trunk/motion_planning/kinematic_planning/include/RKPSpaceInformation.h
pkg/trunk/motion_planning/kinematic_planning/include/RKPStateValidator.h
pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
pkg/trunk/robot_srvs/srv/ValidateKinematicPath.srv
Modified: pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt 2008-09-09 17:50:37 UTC (rev 4095)
+++ pkg/trunk/motion_planning/kinematic_planning/CMakeLists.txt 2008-09-09 17:51:02 UTC (rev 4096)
@@ -2,5 +2,8 @@
include(rosbuild)
rospack(kinematic_planning)
rospack_add_executable(kinematic_planning src/kinematic_planning.cpp)
+rospack_add_executable(motion_validator src/motion_validator.cpp)
+rospack_add_link_flags(kinematic_planning)
+rospack_add_link_flags(motion_validator)
+
add_subdirectory(test)
-rospack_add_link_flags(kinematic_planning)
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h 2008-09-09 17:50:37 UTC (rev 4095)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h 2008-09-09 17:51:02 UTC (rev 4096)
@@ -118,10 +118,10 @@
/* set the workspace volume for planning */
// only area or volume should go through... not sure what happens on really complicated models where
// we have both multiple planar and multiple floating joints...
- static_cast<RKPPlannerSetup::SpaceInformationRKPModel*>(psetup->si)->setPlanningArea(params.volumeMin.x, params.volumeMin.y,
- params.volumeMax.x, params.volumeMax.y);
- static_cast<RKPPlannerSetup::SpaceInformationRKPModel*>(psetup->si)->setPlanningVolume(params.volumeMin.x, params.volumeMin.y, params.volumeMin.z,
- params.volumeMax.x, params.volumeMax.y, params.volumeMax.z);
+ static_cast<SpaceInformationRKPModel*>(psetup->si)->setPlanningArea(params.volumeMin.x, params.volumeMin.y,
+ params.volumeMax.x, params.volumeMax.y);
+ static_cast<SpaceInformationRKPModel*>(psetup->si)->setPlanningVolume(params.volumeMin.x, params.volumeMin.y, params.volumeMin.z,
+ params.volumeMax.x, params.volumeMax.y, params.volumeMax.z);
psetup->si->setStateDistanceEvaluator(psetup->sde[params.distance_metric]);
@@ -152,7 +152,7 @@
/** Set the kinematic constraints to follow */
void setupPoseConstraints(RKPPlannerSetup *psetup, const std::vector<robot_msgs::PoseConstraint> &cstrs)
{
- static_cast<RKPPlannerSetup::StateValidityPredicate*>(psetup->si->getStateValidityChecker())->setPoseConstraints(cstrs);
+ static_cast<StateValidityPredicate*>(psetup->si->getStateValidityChecker())->setPoseConstraints(cstrs);
}
/** Compute the actual motion plan */
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPPlannerSetup.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPPlannerSetup.h 2008-09-09 17:50:37 UTC (rev 4095)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPPlannerSetup.h 2008-09-09 17:51:02 UTC (rev 4096)
@@ -37,23 +37,20 @@
#ifndef KINEMATIC_PLANNING_RKP_PLANNER_SETUP_
#define KINEMATIC_PLANNING_RKP_PLANNER_SETUP_
+#include "RKPModelBase.h"
+#include "RKPStateValidator.h"
+#include "RKPSpaceInformation.h"
+
#include <ompl/base/Planner.h>
-#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
#include <ompl/extension/samplingbased/kinematic/PathSmootherKinematic.h>
-#include <planning_models/kinematic.h>
#include <string_utils/string_utils.h>
-
#include <cassert>
-#include <iostream>
#include <vector>
#include <string>
#include <map>
-#include "RKPModelBase.h"
-#include "RKPConstraintEvaluator.h"
-
class RKPPlannerSetup
{
public:
@@ -81,153 +78,6 @@
delete si;
}
- /** This class configures an instance of SpaceInformationKinematic with data from a KinematicModel */
- class SpaceInformationRKPModel : public ompl::SpaceInformationKinematic
- {
- public:
- SpaceInformationRKPModel(RKPModelBase *model, double divisions = 20.0) : SpaceInformationKinematic()
- {
- m_kmodel = model->kmodel;
- m_groupID = model->groupID;
- m_divisions = divisions;
-
- /* compute the state space for this group */
- m_stateDimension = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID].size() : m_kmodel->stateDimension;
- m_stateComponent.resize(m_stateDimension);
-
- for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
- {
- if (m_stateComponent[i].type == StateComponent::UNKNOWN)
- m_stateComponent[i].type = StateComponent::NORMAL;
- int p = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID][i] * 2 : i * 2;
- m_stateComponent[i].minValue = m_kmodel->stateBounds[p ];
- m_stateComponent[i].maxValue = m_kmodel->stateBounds[p + 1];
- m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / m_divisions;
-
- for (unsigned int j = 0 ; j < m_kmodel->floatingJoints.size() ; ++j)
- if (m_kmodel->floatingJoints[j] == p)
- {
- m_floatingJoints.push_back(i);
- m_stateComponent[i + 3].type = StateComponent::QUATERNION;
- break;
- }
-
- for (unsigned int j = 0 ; j < m_kmodel->planarJoints.size() ; ++j)
- if (m_kmodel->planarJoints[j] == p)
- {
- m_planarJoints.push_back(i);
- break;
- }
- }
- updateResolution();
- }
-
- virtual ~SpaceInformationRKPModel(void)
- {
- }
-
- /** For planar and floating joints, we have infinite
- dimensions. The bounds for these dimensions are set by the
- user. */
- void setPlanningVolume(double x0, double y0, double z0, double x1, double y1, double z1)
- {
- for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
- {
- int id = m_floatingJoints[i];
- m_stateComponent[id ].minValue = x0;
- m_stateComponent[id ].maxValue = x1;
- m_stateComponent[id + 1].minValue = y0;
- m_stateComponent[id + 1].maxValue = y1;
- m_stateComponent[id + 2].minValue = z0;
- m_stateComponent[id + 2].maxValue = z1;
- for (int j = 0 ; j < 3 ; ++j)
- m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
- }
- updateResolution();
- }
-
- void setPlanningArea(double x0, double y0, double x1, double y1)
- {
- for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
- {
- int id = m_planarJoints[i];
- m_stateComponent[id ].minValue = x0;
- m_stateComponent[id ].maxValue = x1;
- m_stateComponent[id + 1].minValue = y0;
- m_stateComponent[id + 1].maxValue = y1;
- for (int j = 0 ; j < 2 ; ++j)
- m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
- }
- updateResolution();
- }
-
- protected:
-
- void updateResolution(void)
- {
- /* for movement in plane/space, we want to make sure the resolution is small enough */
- for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
- {
- if (m_stateComponent[m_planarJoints[i]].resolution > 0.1)
- m_stateComponent[m_planarJoints[i]].resolution = 0.1;
- if (m_stateComponent[m_planarJoints[i] + 1].resolution > 0.1)
- m_stateComponent[m_planarJoints[i] + 1].resolution = 0.1;
- }
- for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
- {
- if (m_stateComponent[m_floatingJoints[i]].resolution > 0.1)
- m_stateComponent[m_floatingJoints[i]].resolution = 0.1;
- if (m_stateComponent[m_floatingJoints[i] + 1].resolution > 0.1)
- m_stateComponent[m_floatingJoints[i] + 1].resolution = 0.1;
- if (m_stateComponent[m_floatingJoints[i] + 2].resolution > 0.1)
- m_stateComponent[m_floatingJoints[i] + 2].resolution = 0.1;
- }
- }
-
- double m_divisions;
- planning_models::KinematicModel *m_kmodel;
- int m_groupID;
- std::vector<int> m_floatingJoints;
- std::vector<int> m_planarJoints;
-
- };
-
- class StateValidityPredicate : public ompl::SpaceInformation::StateValidityChecker
- {
- public:
- StateValidityPredicate(RKPModelBase *model) : ompl::SpaceInformation::StateValidityChecker()
- {
- m_model = model;
- }
-
- virtual bool operator()(const ompl::SpaceInformation::State_t state)
- {
- m_model->kmodel->computeTransforms(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
- m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
-
- bool valid = !m_model->collisionSpace->isCollision(m_model->collisionSpaceID);
-
- if (valid)
- valid = m_kce.decide();
-
- return valid;
- }
-
- void setPoseConstraints(const std::vector<robot_msgs::PoseConstraint> &kc)
- {
- m_kce.use(m_model->kmodel, kc);
- }
-
- void clearConstraints(void)
- {
- m_kce.clear();
- }
-
- protected:
- RKPModelBase *m_model;
- KinematicConstraintEvaluatorSet m_kce;
- };
-
/* for each planner definition, define the set of distance metrics it can use */
virtual void setupDistanceEvaluators(void)
{
Added: pkg/trunk/motion_planning/kinematic_planning/include/RKPSpaceInformation.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPSpaceInformation.h (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPSpaceInformation.h 2008-09-09 17:51:02 UTC (rev 4096)
@@ -0,0 +1,156 @@
+/*********************************************************************
+* 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 KINEMATIC_PLANNING_RKP_SPACE_INFORMATION_
+#define KINEMATIC_PLANNING_RKP_SPACE_INFORMATION_
+
+#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
+#include "RKPModelBase.h"
+
+#include <vector>
+
+/** This class configures an instance of SpaceInformationKinematic with data from a KinematicModel */
+class SpaceInformationRKPModel : public ompl::SpaceInformationKinematic
+{
+ public:
+ SpaceInformationRKPModel(RKPModelBase *model, double divisions = 20.0) : SpaceInformationKinematic()
+ {
+ m_kmodel = model->kmodel;
+ m_groupID = model->groupID;
+ m_divisions = divisions;
+
+ /* compute the state space for this group */
+ m_stateDimension = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID].size() : m_kmodel->stateDimension;
+ m_stateComponent.resize(m_stateDimension);
+
+ for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ {
+ if (m_stateComponent[i].type == StateComponent::UNKNOWN)
+ m_stateComponent[i].type = StateComponent::NORMAL;
+ int p = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID][i] * 2 : i * 2;
+ m_stateComponent[i].minValue = m_kmodel->stateBounds[p ];
+ m_stateComponent[i].maxValue = m_kmodel->stateBounds[p + 1];
+ m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / m_divisions;
+
+ for (unsigned int j = 0 ; j < m_kmodel->floatingJoints.size() ; ++j)
+ if (m_kmodel->floatingJoints[j] == p)
+ {
+ m_floatingJoints.push_back(i);
+ m_stateComponent[i + 3].type = StateComponent::QUATERNION;
+ break;
+ }
+
+ for (unsigned int j = 0 ; j < m_kmodel->planarJoints.size() ; ++j)
+ if (m_kmodel->planarJoints[j] == p)
+ {
+ m_planarJoints.push_back(i);
+ break;
+ }
+ }
+ updateResolution();
+ }
+
+ virtual ~SpaceInformationRKPModel(void)
+ {
+ }
+
+ /** For planar and floating joints, we have infinite
+ dimensions. The bounds for these dimensions are set by the
+ user. */
+ void setPlanningVolume(double x0, double y0, double z0, double x1, double y1, double z1)
+ {
+ for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
+ {
+ int id = m_floatingJoints[i];
+ m_stateComponent[id ].minValue = x0;
+ m_stateComponent[id ].maxValue = x1;
+ m_stateComponent[id + 1].minValue = y0;
+ m_stateComponent[id + 1].maxValue = y1;
+ m_stateComponent[id + 2].minValue = z0;
+ m_stateComponent[id + 2].maxValue = z1;
+ for (int j = 0 ; j < 3 ; ++j)
+ m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
+ }
+ updateResolution();
+ }
+
+ void setPlanningArea(double x0, double y0, double x1, double y1)
+ {
+ for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
+ {
+ int id = m_planarJoints[i];
+ m_stateComponent[id ].minValue = x0;
+ m_stateComponent[id ].maxValue = x1;
+ m_stateComponent[id + 1].minValue = y0;
+ m_stateComponent[id + 1].maxValue = y1;
+ for (int j = 0 ; j < 2 ; ++j)
+ m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
+ }
+ updateResolution();
+ }
+
+ protected:
+
+ void updateResolution(void)
+ {
+ /* for movement in plane/space, we want to make sure the resolution is small enough */
+ for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
+ {
+ if (m_stateComponent[m_planarJoints[i]].resolution > 0.1)
+ m_stateComponent[m_planarJoints[i]].resolution = 0.1;
+ if (m_stateComponent[m_planarJoints[i] + 1].resolution > 0.1)
+ m_stateComponent[m_planarJoints[i] + 1].resolution = 0.1;
+ }
+ for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
+ {
+ if (m_stateComponent[m_floatingJoints[i]].resolution > 0.1)
+ m_stateComponent[m_floatingJoints[i]].resolution = 0.1;
+ if (m_stateComponent[m_floatingJoints[i] + 1].resolution > 0.1)
+ m_stateComponent[m_floatingJoints[i] + 1].resolution = 0.1;
+ if (m_stateComponent[m_floatingJoints[i] + 2].resolution > 0.1)
+ m_stateComponent[m_floatingJoints[i] + 2].resolution = 0.1;
+ }
+ }
+
+ double m_divisions;
+ planning_models::KinematicModel *m_kmodel;
+ int m_groupID;
+ std::vector<int> m_floatingJoints;
+ std::vector<int> m_planarJoints;
+
+};
+
+#endif
Added: pkg/trunk/motion_planning/kinematic_planning/include/RKPStateValidator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPStateValidator.h (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPStateValidator.h 2008-09-09 17:51:02 UTC (rev 4096)
@@ -0,0 +1,82 @@
+/*********************************************************************
+* 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 KINEMATIC_PLANNING_RKP_STATE_VALIDATOR
+#define KINEMATIC_PLANNING_RKP_STATE_VALIDATOR
+
+#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
+#include <planning_models/kinematic.h>
+#include <collision_space/environment.h>
+#include "RKPModelBase.h"
+#include "RKPConstraintEvaluator.h"
+
+class StateValidityPredicate : public ompl::SpaceInformation::StateValidityChecker
+{
+ public:
+ StateValidityPredicate(RKPModelBase *model) : ompl::SpaceInformation::StateValidityChecker()
+ {
+ m_model = model;
+ }
+
+ virtual bool operator()(const ompl::SpaceInformation::State_t state)
+ {
+ m_model->kmodel->computeTransforms(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
+ m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
+
+ bool valid = !m_model->collisionSpace->isCollision(m_model->collisionSpaceID);
+
+ if (valid)
+ valid = m_kce.decide();
+
+ return valid;
+ }
+
+ void setPoseConstraints(const std::vector<robot_msgs::PoseConstraint> &kc)
+ {
+ m_kce.use(m_model->kmodel, kc);
+ }
+
+ void clearConstraints(void)
+ {
+ m_kce.clear();
+ }
+
+ protected:
+ RKPModelBase *m_model;
+ KinematicConstraintEvaluatorSet m_kce;
+};
+
+#endif
Added: pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/src/motion_validator.cpp 2008-09-09 17:51:02 UTC (rev 4096)
@@ -0,0 +1,292 @@
+/*********************************************************************
+* 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 */
+
+/**
+
+@mainpage
+
+@htmlinclude ../manifest.html
+
+@b MotionValidator is a node capable of verifying if a path is valid
+or not (collides or does not collide).
+
+<hr>
+
+@section usage Usage
+@verbatim
+$ motion_validator robot_model [standard ROS args]
+@endverbatim
+
+@par Example
+
+@verbatim
+$ motion_validator robotdesc/pr2
+@endverbatim
+
+<hr>
+
+@section topic ROS topics
+
+Subscribes to (name/type):
+- None
+
+Additional subscriptions due to inheritance from NodeCollisionModel:
+- @b localizedpose/RobotBase2DOdom : localized position of the robot base
+- @b world_3d_map/PointCloudFloat32 : point cloud with data describing the 3D environment
+
+Publishes to (name/type):
+- None
+
+<hr>
+
+@section services ROS services
+
+Uses (name/type):
+- None
+
+Provides (name/type):
+- @b "validate_path"/KinematicPlanState : given a robot model, starting and goal states, this service computes a collision free path
+
+
+<hr>
+
+@section parameters ROS parameters
+- None
+
+**/
+
+#include <planning_node_util/cnode.h>
+#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
+
+#include <robot_srvs/ValidateKinematicPath.h>
+
+#include "RKPStateValidator.h"
+#include "RKPSpaceInformation.h"
+
+#include <iostream>
+#include <string>
+#include <map>
+
+class MotionValidator : public ros::node,
+ public planning_node_util::NodeCollisionModel
+{
+public:
+
+ class myModel : public RKPModelBase
+ {
+ public:
+ myModel(void) : RKPModelBase()
+ {
+ si = NULL;
+ svc = NULL;
+ }
+
+ virtual ~myModel(void)
+ {
+ if (svc)
+ delete svc;
+ if (si)
+ delete si;
+ }
+
+ ompl::SpaceInformationKinematic_t si;
+ ompl::SpaceInformation::StateValidityChecker_t svc;
+ };
+
+ MotionValidator(const std::string &robot_model) : ros::node("kinematic_planning"),
+ planning_node_util::NodeCollisionModel(dynamic_cast<ros::node*>(this),
+ robot_model)
+ {
+ advertise_service("validate_path", &MotionValidator::validatePath);
+ }
+
+ /** Free the memory */
+ virtual ~MotionValidator(void)
+ {
+ for (std::map<std::string, myModel*>::iterator i = m_models.begin() ; i != m_models.end() ; i++)
+ delete i->second;
+ }
+
+ bool validatePath(robot_srvs::ValidateKinematicPath::request &req, robot_srvs::ValidateKinematicPath::response &res)
+ {
+ myModel *model = m_models[req.model_id];
+ if (model)
+ {
+ if (model->kmodel->stateDimension != req.start_state.get_vals_size())
+ {
+ std::cerr << "Dimension of start state expected to be " << model->kmodel->stateDimension << " but was received as " << req.start_state.get_vals_size() << std::endl;
+ return false;
+ }
+
+ if (model->si->getStateDimension() != req.goal_state.get_vals_size())
+ {
+ std::cerr << "Dimension of start goal expected to be " << model->si->getStateDimension() << " but was received as " << req.goal_state.get_vals_size() << std::endl;
+ return false;
+ }
+
+ std::cout << "Validating path for '" << req.model_id << "'..." << std::endl;
+
+ const unsigned int dim = model->si->getStateDimension();
+ ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
+
+ if (model->groupID >= 0)
+ {
+ /* set the pose of the whole robot */
+ model->kmodel->computeTransforms(req.start_state.vals);
+ model->collisionSpace->updateRobotModel(model->collisionSpaceID);
+
+ /* extract the components needed for the start state of the desired group */
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ start->values[i] = req.start_state.vals[model->kmodel->groupStateIndexList[model->groupID][i]];
+ }
+ else
+ {
+ /* the start state is the complete state */
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ start->values[i] = req.start_state.vals[i];
+ }
+
+ ompl::SpaceInformationKinematic::StateKinematic_t goal = new ompl::SpaceInformationKinematic::StateKinematic(dim);
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ goal->values[i] = req.goal_state.vals[i];
+
+
+ res.valid = model->si->checkMotionIncremental(start, goal);
+
+ std::cout << "Result: " << res.valid << std::endl;
+
+ delete start;
+ delete goal;
+
+ return true;
+ }
+ else
+ {
+ std::cerr << "Model '" << req.model_id << "' not known" << std::endl;
+ return false;
+ }
+ }
+
+ virtual void setRobotDescription(robot_desc::URDF *file)
+ {
+ planning_node_util::NodeCollisionModel::setRobotDescription(file);
+
+ printf("=======================================\n");
+ m_kmodel->printModelInfo();
+ printf("=======================================\n");
+
+ /* set the data for the model */
+ myModel *model = new myModel();
+ model->collisionSpaceID = 0;
+ model->collisionSpace = m_collisionSpace;
+ model->kmodel = m_kmodel;
+ model->groupName = m_kmodel->name;
+ setupModel(model);
+
+ /* remember the model by the robot's name */
+ m_models[model->groupName] = model;
+
+ /* create a model for each group */
+ std::vector<std::string> groups;
+ m_kmodel->getGroups(groups);
+
+ for (unsigned int i = 0 ; i < groups.size() ; ++i)
+ {
+ myModel *model = new myModel();
+ model->collisionSpaceID = 0;
+ model->collisionSpace = m_collisionSpace;
+ model->kmodel = m_kmodel;
+ model->groupID = m_kmodel->getGroupID(groups[i]);
+ model->groupName = groups[i];
+ setupModel(model);
+ m_models[model->groupName] = model;
+ }
+ }
+
+ void knownModels(std::vector<std::string> &model_ids)
+ {
+ for (std::map<std::string, myModel*>::const_iterator i = m_models.begin() ; i != m_models.end() ; ++i)
+ model_ids.push_back(i->first);
+ }
+
+private:
+
+ void setupModel(myModel *model)
+ {
+ model->si = new SpaceInformationRKPModel(model);
+ model->svc = new StateValidityPredicate(model);
+ model->si->setStateValidityChecker(model->svc);
+ }
+
+ std::map<std::string, myModel*> m_models;
+
+
+};
+
+void usage(const char *progname)
+{
+ printf("\nUsage: %s robot_model [standard ROS args]\n", progname);
+ printf(" \"robot_model\" is the name (string) of a robot description to be used for path validation.\n");
+}
+
+int main(int argc, char **argv)
+{
+ if (argc == 2)
+ {
+ ros::init(argc, argv);
+
+ MotionValidator *validator = new MotionValidator(argv[1]);
+ validator->loadRobotDescription();
+
+ std::vector<std::string> mlist;
+ validator->knownModels(mlist);
+ printf("Known models:\n");
+ for (unsigned int i = 0 ; i < mlist.size() ; ++i)
+ printf(" * %s\n", mlist[i].c_str());
+ if (mlist.size() > 0)
+ validator->spin();
+ else
+ printf("No models defined. Path validation node cannot start.\n");
+
+ validator->shutdown();
+
+ delete validator;
+ }
+ else
+ usage(argv[0]);
+
+ return 0;
+}
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2008-09-09 17:50:37 UTC (rev 4095)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2008-09-09 17:51:02 UTC (rev 4096)
@@ -15,9 +15,7 @@
# The goal state for the model to plan for. The dimension of this
# state must be equal to the dimension of the state space
-# characterizing the model (group) to plan for. If this state has
-# dimension 0, it is assumed that the first state that satisfies the
-# constraints is correct
+# characterizing the model (group) to plan for.
robot_msgs/KinematicState goal_state
# No state in the produced motion plan will violate these constraints
Added: pkg/trunk/robot_srvs/srv/ValidateKinematicPath.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/ValidateKinematicPath.srv (rev 0)
+++ pkg/trunk/robot_srvs/srv/ValidateKinematicPath.srv 2008-09-09 17:51:02 UTC (rev 4096)
@@ -0,0 +1,27 @@
+# This message contains the definition for a request to the state
+# validator
+
+# The model to validate for
+string model_id
+
+# The starting state for the robot. This is eth complete state of the
+# robot, all joint values, everything that needs to be specified to
+# completely define where each part of the robot is in space. The
+# meaning for each element in the state vector can be extracted from
+# viewing the output of the robot model construction (the kinematic
+# model constructed from the parsed URDF model) in verbose mode
+robot_msgs/KinematicState start_state
+
+
+# The goal state for the model to validate for. The dimension of this
+# state must be equal to the dimension of the state space
+# characterizing the model (group) to plan for.
+robot_msgs/KinematicState goal_state
+
+# No state in the produced motion plan will violate these constraints
+robot_msgs/KinematicConstraints constraints
+
+---
+
+# Return true or false, to say if path is valid or not.
+byte valid
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
