Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3346] pkg/trunk
|
From: <is...@us...> - 2008-08-20 20:29:41
|
Revision: 3346
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3346&view=rev
Author: isucan
Date: 2008-08-20 20:29:42 +0000 (Wed, 20 Aug 2008)
Log Message:
-----------
fixed some issues with the kinematic model construction (I interpreted some input incorrectly and multiplied a transform in incorrect order), added a collision space with octrees (incomplete, not yet tested), fixed some issues with motion planning (mostly organizing things better), some improved documentation
Modified Paths:
--------------
pkg/trunk/motion_planning/collision_space/CMakeLists.txt
pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h
pkg/trunk/motion_planning/collision_space/include/collision_space/util.h
pkg/trunk/motion_planning/collision_space/manifest.xml
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/motion_planning/collision_space/test/test_util.cpp
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/LazyRRT.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/KinematicPathSmoother.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
pkg/trunk/motion_planning/test_collision_space/src/test_kinematic_ode.cpp
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
Modified: pkg/trunk/motion_planning/collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/collision_space/CMakeLists.txt 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/collision_space/CMakeLists.txt 2008-08-20 20:29:42 UTC (rev 3346)
@@ -1,7 +1,9 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(collision_space)
-rospack_add_library(collision_space src/collision_space/environment.cpp src/collision_space/environmentODE.cpp)
+rospack_add_library(collision_space src/collision_space/environment.cpp
+# src/collision_space/environmentOctree.cpp
+ src/collision_space/environmentODE.cpp)
# Unit tests
rospack_add_gtest(test_util test/test_util.cpp)
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-08-20 20:29:42 UTC (rev 3346)
@@ -163,7 +163,8 @@
dSpaceID s;
};
- void freeMemory(void);
+ dGeomID createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape) const;
+ void freeMemory(void);
std::vector<ModelInfo> m_kgeoms;
dSpaceID m_space;
Added: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h (rev 0)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-08-20 20:29:42 UTC (rev 3346)
@@ -0,0 +1,91 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+#ifndef COLLISION_SPACE_ENVIRONMENT_MODEL_OCTREE_
+#define COLLISION_SPACE_ENVIRONMENT_MODEL_OCTREE_
+
+#include <collision_space/environment.h>
+#include <octree.h>
+#include <vector>
+
+/** @htmlinclude ../../manifest.html
+
+ A class describing an environment for a kinematic robot using ODE */
+
+namespace collision_space
+{
+
+ static const char OCTREE_CELL_EMPTY = 0;
+ static const char OCTREE_CELL_OCCUPIED = 1;
+
+ class EnvironmentModelOctree : public EnvironmentModel
+ {
+ public:
+
+ EnvironmentModelOctree(void) : EnvironmentModel(),
+ m_octree(0.0f, 0.0f, 0.0f, 80.0f, 80.0f, 80.0f, 12, OCTREE_CELL_EMPTY)
+ {
+ }
+
+ ~EnvironmentModelOctree(void)
+ {
+ }
+
+
+ /** Check if a model is in collision */
+ virtual bool isCollision(unsigned int model_id);
+
+ /** Remove all obstacles from collision model */
+ virtual void clearObstacles(void);
+
+ /** Add a point cloud to the collision space */
+ virtual void addPointCloud(unsigned int n, const double *points, double radius = 0.01);
+
+ /** Add a robot model */
+ virtual unsigned int addRobotModel(planning_models::KinematicModel *model);
+
+ /** Update the positions of the geometry used in collision detection */
+ virtual void updateRobotModel(unsigned int model_id);
+
+ const scan_utils::Octree<char>* getOctree(void) const;
+
+ protected:
+
+ scan_utils::Octree<char> m_octree;
+ std::vector<std::vector< planning_models::KinematicModel::Link*> > m_modelLinks;
+
+ };
+}
+
+#endif
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/util.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/util.h 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/util.h 2008-08-20 20:29:42 UTC (rev 3346)
@@ -44,15 +44,15 @@
namespace bodies
{
- class Object
+ class Shape
{
public:
- Object(void)
+ Shape(void)
{
m_scale = 1.0;
}
- virtual ~Object(void)
+ virtual ~Shape(void)
{
}
@@ -92,10 +92,10 @@
};
- class Sphere : public Object
+ class Sphere : public Shape
{
public:
- Sphere(void) : Object()
+ Sphere(void) : Shape()
{
m_radius = 0.0;
}
@@ -132,10 +132,10 @@
};
- class Cylinder : public Object
+ class Cylinder : public Shape
{
public:
- Cylinder(void) : Object()
+ Cylinder(void) : Shape()
{
m_length = m_radius = 0.0;
}
@@ -198,10 +198,10 @@
};
- class Box : public Object
+ class Box : public Shape
{
public:
- Box(void) : Object()
+ Box(void) : Shape()
{
m_length = m_width = m_height = 0.0;
}
Modified: pkg/trunk/motion_planning/collision_space/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/collision_space/manifest.xml 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/collision_space/manifest.xml 2008-08-20 20:29:42 UTC (rev 3346)
@@ -8,6 +8,7 @@
<depend package="rosthread"/>
<depend package="planning_models"/>
+ <depend package="scan_utils"/>
<depend package="opende"/>
<depend package="gtest"/>
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-08-20 20:29:42 UTC (rev 3346)
@@ -63,22 +63,7 @@
{
kGeom *kg = new kGeom();
kg->link = robot->links[i];
- planning_models::KinematicModel::Geometry *geom = robot->links[i]->geom;
- dGeomID g = NULL;
- switch (geom->type)
- {
- case planning_models::KinematicModel::Geometry::SPHERE:
- g = dCreateSphere(m_kgeoms[id].s, geom->size[0]);
- break;
- case planning_models::KinematicModel::Geometry::BOX:
- g = dCreateBox(m_kgeoms[id].s, geom->size[0], geom->size[1], geom->size[2]);
- break;
- case planning_models::KinematicModel::Geometry::CYLINDER:
- g = dCreateCylinder(m_kgeoms[id].s, geom->size[0], geom->size[1]);
- break;
- default:
- break;
- }
+ dGeomID g = createODEGeom(m_kgeoms[id].s, robot->links[i]->shape);
if (g)
{
kg->geom = g;
@@ -91,6 +76,34 @@
return id;
}
+dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape) const
+{
+ dGeomID g = NULL;
+ switch (shape->type)
+ {
+ case planning_models::KinematicModel::Shape::SPHERE:
+ {
+ g = dCreateSphere(space, static_cast<planning_models::KinematicModel::Sphere*>(shape)->radius);
+ }
+ break;
+ case planning_models::KinematicModel::Shape::BOX:
+ {
+ const double *size = static_cast<planning_models::KinematicModel::Box*>(shape)->size;
+ g = dCreateBox(space, size[0], size[1], size[2]);
+ }
+ break;
+ case planning_models::KinematicModel::Shape::CYLINDER:
+ {
+ g = dCreateCylinder(space, static_cast<planning_models::KinematicModel::Cylinder*>(shape)->radius,
+ static_cast<planning_models::KinematicModel::Cylinder*>(shape)->length);
+ }
+ break;
+ default:
+ break;
+ }
+ return g;
+}
+
void collision_space::EnvironmentModelODE::updateRobotModel(unsigned int model_id)
{
const unsigned int n = m_kgeoms[model_id].g.size();
Added: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp (rev 0)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-08-20 20:29:42 UTC (rev 3346)
@@ -0,0 +1,134 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+#include <collision_space/environmentOctree.h>
+
+unsigned int collision_space::EnvironmentModelOctree::addRobotModel(planning_models::KinematicModel *model)
+{
+ unsigned int id = collision_space::EnvironmentModel::addRobotModel(model);
+ if (id <= m_modelLinks.size())
+ m_modelLinks.resize(id + 1);
+ m_models[id]->getLinks(m_modelLinks[id]);
+ return id;
+}
+
+void collision_space::EnvironmentModelOctree::updateRobotModel(unsigned int model_id)
+{
+}
+
+bool collision_space::EnvironmentModelOctree::isCollision(unsigned int model_id)
+{
+ bool result = false;
+
+ const std::vector<planning_models::KinematicModel::Link*> &links = m_modelLinks[model_id];
+
+ for (unsigned int i = 0 ; !result && i < links.size() ; ++i)
+ {
+ switch (links[i]->geom->type)
+ {
+ case planning_models::KinematicModel::Geometry::BOX:
+ {
+ NEWMAT::Matrix mat = links[i]->globalTrans.asMatrix();
+ float axes[3][3];
+
+ for (unsigned int j=0; j<3; j++)
+ for (unsigned int k=0; k<3; k++)
+ axes[j][k] = mat.element(j,k);
+
+ libTF::Pose3D::Position libTFpos;
+ links[i]->globalTrans.getPosition(libTFpos);
+ float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z };
+ const double *size = links[i]->geom->size;
+ const float sizef[3] = {size[0], size[1], size[2]};
+
+ result = m_octree.intersectsBox(pos, sizef, axes);
+ }
+
+ break;
+
+ case planning_models::KinematicModel::Geometry::CYLINDER:
+ {
+ NEWMAT::Matrix mat = links[i]->globalTrans.asMatrix();
+ float axes[3][3];
+
+ for (unsigned int j=0; j<3; j++)
+ for (unsigned int k=0; k<3; k++)
+ axes[j][k] = mat.element(j,k);
+
+ libTF::Pose3D::Position libTFpos;
+ links[i]->globalTrans.getPosition(libTFpos);
+ float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z };
+ const double *size = links[i]->geom->size;
+ const float L = size[1]*sqrt(2);
+ const float sizef[3] = {size[0], L, L};
+
+ result = m_octree.intersectsBox(pos, sizef, axes);
+ }
+ break;
+
+ case planning_models::KinematicModel::Geometry::SPHERE:
+ {
+ libTF::Pose3D::Position libTFpos;
+ links[i]->globalTrans.getPosition(libTFpos);
+ float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z };
+ result = m_octree.intersectsSphere(pos, links[i]->geom->size[0]);
+ }
+ break;
+ default:
+ fprintf(stderr, "Geometry type not implemented: %d\n", links[i]->geom->type);
+ break;
+ }
+ }
+
+ return result;
+}
+
+void collision_space::EnvironmentModelOctree::addPointCloud(unsigned int n, const double *points, double radius)
+{
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ unsigned int i3 = i * 3;
+ m_octree.insert(points[i3], points[i3 + 1], points[i3 + 2], OCTREE_CELL_OCCUPIED);
+ }
+}
+
+void collision_space::EnvironmentModelOctree::clearObstacles(void)
+{
+ m_octree.clear();
+}
+
+const scan_utils::Octree<char>* collision_space::EnvironmentModelOctree::getOctree(void) const
+{
+ return &m_octree;
+}
Modified: pkg/trunk/motion_planning/collision_space/test/test_util.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/test/test_util.cpp 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/collision_space/test/test_util.cpp 2008-08-20 20:29:42 UTC (rev 3346)
@@ -37,7 +37,7 @@
TEST(SpherePointContainment, SimpleInside)
{
- collision_space::bodies::Object* sphere = new collision_space::bodies::Sphere();
+ collision_space::bodies::Shape* sphere = new collision_space::bodies::Sphere();
double dims = 1.0;
sphere->setDimensions(&dims);
sphere->setScale(1.05);
@@ -48,7 +48,7 @@
TEST(SpherePointContainment, SimpleOutside)
{
- collision_space::bodies::Object* sphere = new collision_space::bodies::Sphere();
+ collision_space::bodies::Shape* sphere = new collision_space::bodies::Sphere();
double dims = 1.0;
sphere->setDimensions(&dims);
sphere->setScale(0.95);
@@ -59,7 +59,7 @@
TEST(SpherePointContainment, ComplexInside)
{
- collision_space::bodies::Object* sphere = new collision_space::bodies::Sphere();
+ collision_space::bodies::Shape* sphere = new collision_space::bodies::Sphere();
double dims = 1.0;
sphere->setDimensions(&dims);
sphere->setScale(0.95);
@@ -73,7 +73,7 @@
TEST(SpherePointContainment, ComplexOutside)
{
- collision_space::bodies::Object* sphere = new collision_space::bodies::Sphere();
+ collision_space::bodies::Shape* sphere = new collision_space::bodies::Sphere();
double dims = 1.0;
sphere->setDimensions(&dims);
sphere->setScale(0.95);
@@ -88,7 +88,7 @@
TEST(BoxPointContainment, SimpleInside)
{
- collision_space::bodies::Object* box = new collision_space::bodies::Box();
+ collision_space::bodies::Shape* box = new collision_space::bodies::Box();
double dims[3] = {1.0, 2.0, 3.0};
box->setDimensions(dims);
box->setScale(0.95);
@@ -100,7 +100,7 @@
TEST(BoxPointContainment, SimpleOutside)
{
- collision_space::bodies::Object* box = new collision_space::bodies::Box();
+ collision_space::bodies::Shape* box = new collision_space::bodies::Box();
double dims[3] = {1.0, 2.0, 3.0};
box->setDimensions(dims);
box->setScale(0.95);
@@ -112,7 +112,7 @@
TEST(BoxPointContainment, ComplexInside)
{
- collision_space::bodies::Object* box = new collision_space::bodies::Box();
+ collision_space::bodies::Shape* box = new collision_space::bodies::Box();
double dims[3] = {1.0, 1.0, 1.0};
box->setDimensions(dims);
box->setScale(1.01);
@@ -128,7 +128,7 @@
TEST(BoxPointContainment, ComplexOutside)
{
- collision_space::bodies::Object* box = new collision_space::bodies::Box();
+ collision_space::bodies::Shape* box = new collision_space::bodies::Box();
double dims[3] = {1.0, 1.0, 1.0};
box->setDimensions(dims);
box->setScale(1.01);
@@ -144,7 +144,7 @@
TEST(CylinderPointContainment, SimpleInside)
{
- collision_space::bodies::Object* cylinder = new collision_space::bodies::Cylinder();
+ collision_space::bodies::Shape* cylinder = new collision_space::bodies::Cylinder();
double dims[3] = {4.0, 1.0};
cylinder->setDimensions(dims);
cylinder->setScale(1.05);
@@ -156,7 +156,7 @@
TEST(CylinderPointContainment, SimpleOutside)
{
- collision_space::bodies::Object* cylinder = new collision_space::bodies::Cylinder();
+ collision_space::bodies::Shape* cylinder = new collision_space::bodies::Cylinder();
double dims[3] = {4.0, 1.0};
cylinder->setDimensions(dims);
cylinder->setScale(0.95);
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-20 20:29:42 UTC (rev 3346)
@@ -61,7 +61,7 @@
Subscribes to (name/type):
- None
-Additional subscriptions due to inheritance from NodeODECollisionModel:
+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
@@ -100,12 +100,13 @@
#include <map>
class KinematicPlanning : public ros::node,
- public planning_node_util::NodeODECollisionModel
+ public planning_node_util::NodeCollisionModel
{
public:
KinematicPlanning(const std::string &robot_model) : ros::node("kinematic_planning"),
- planning_node_util::NodeODECollisionModel(dynamic_cast<ros::node*>(this), robot_model)
+ planning_node_util::NodeCollisionModel(dynamic_cast<ros::node*>(this),
+ robot_model)
{
advertise_service("plan_kinematic_path", &KinematicPlanning::plan);
}
@@ -233,7 +234,7 @@
virtual void setRobotDescription(robot_desc::URDF *file)
{
- planning_node_util::NodeODECollisionModel::setRobotDescription(file);
+ planning_node_util::NodeCollisionModel::setRobotDescription(file);
defaultPosition();
/* set the data for the model */
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-08-20 20:29:42 UTC (rev 3346)
@@ -211,7 +211,8 @@
virtual void sample(StateKinematic_t state);
virtual void sampleNear(StateKinematic_t state, const StateKinematic_t near, double rho);
- virtual bool checkMotion(const StateKinematic_t s1, const StateKinematic_t s2);
+ virtual bool checkMotionSubdivision(const StateKinematic_t s1, const StateKinematic_t s2);
+ virtual bool checkMotionLinearly(const StateKinematic_t s1, const StateKinematic_t s2);
virtual void interpolatePath(PathKinematic_t path);
bool isValid(const StateKinematic_t state)
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/LazyRRT.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/LazyRRT.cpp 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/LazyRRT.cpp 2008-08-20 20:29:42 UTC (rev 3346)
@@ -50,8 +50,6 @@
time_utils::Time endTime = time_utils::Time::now() + time_utils::Duration(solveTime);
- Motion_t __hack = NULL;
-
if (m_nn.size() == 0)
{
for (unsigned int i = 0 ; i < m_si->getStartStateCount() ; ++i)
@@ -60,8 +58,6 @@
si->copyState(motion->state, dynamic_cast<SpaceInformationKinematic::StateKinematic_t>(si->getStartState(i)));
if (si->isValid(motion->state))
{
- __hack = motion;
-
motion->valid = true;
m_nn.add(motion);
}
@@ -141,7 +137,7 @@
for (int i = mpath.size() - 1 ; i >= 0 ; --i)
if (!mpath[i]->valid)
{
- if (si->checkMotion(mpath[i]->parent->state, mpath[i]->state))
+ if (si->checkMotionSubdivision(mpath[i]->parent->state, mpath[i]->state))
mpath[i]->valid = true;
else
{
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/src/RRT.cpp 2008-08-20 20:29:42 UTC (rev 3346)
@@ -105,7 +105,7 @@
si->copyState(motion->state, xstate);
motion->parent = nmotion;
- if (si->checkMotion(nmotion->state, motion->state))
+ if (si->checkMotionSubdivision(nmotion->state, motion->state))
{
m_nn.add(motion);
double dist = goal_r->distanceGoal(motion->state);
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/KinematicPathSmoother.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/KinematicPathSmoother.cpp 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/KinematicPathSmoother.cpp 2008-08-20 20:29:42 UTC (rev 3346)
@@ -67,7 +67,7 @@
if (p1 > p2)
std::swap(p1, p2);
- if (si->checkMotion(path->states[p1], path->states[p2]))
+ if (si->checkMotionSubdivision(path->states[p1], path->states[p2]))
{
for (int i = p1 + 1 ; i < p2 ; ++i)
delete path->states[i];
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2008-08-20 20:29:42 UTC (rev 3346)
@@ -103,7 +103,7 @@
std::min(m_stateComponent[i].maxValue, near->values[i] + rho));
}
-bool ompl::SpaceInformationKinematic::checkMotion(const StateKinematic_t s1, const StateKinematic_t s2)
+bool ompl::SpaceInformationKinematic::checkMotionSubdivision(const StateKinematic_t s1, const StateKinematic_t s2)
{
/* assume motion starts in a valid configuration so s1 is valid */
if (!isValid(s2))
@@ -125,7 +125,7 @@
/* initialize the queue of test positions */
std::queue< std::pair<int, int> > pos;
- if (nd > 2)
+ if (nd >= 2)
pos.push(std::make_pair(1, nd - 1));
/* temporary storage for the checked state */
@@ -155,6 +155,40 @@
return true;
}
+bool ompl::SpaceInformationKinematic::checkMotionLinearly(const StateKinematic_t s1, const StateKinematic_t s2)
+{
+ /* assume motion starts in a valid configuration so s1 is valid */
+ if (!isValid(s2))
+ return false;
+
+ /* find out how many divisions we need */
+ int nd = 1;
+ for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ {
+ int d = 1 + (int)(fabs(s1->values[i] - s2->values[i]) / m_stateComponent[i].resolution);
+ if (nd < d)
+ nd = d;
+ }
+
+ /* find out the step size as a vector */
+ std::valarray<double> step(m_stateDimension);
+ for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ step[i] = (s2->values[i] - s1->values[i]) / (double)nd;
+
+ /* temporary storage for the checked state */
+ StateKinematic test(m_stateDimension);
+
+ for (int j = 1 ; j < nd ; ++j)
+ {
+ for (unsigned int k = 0 ; k < m_stateDimension ; ++k)
+ test.values[k] = s1->values[k] + (double)j * step[k];
+ if (!isValid(&test))
+ return false;
+ }
+
+ return true;
+}
+
void ompl::SpaceInformationKinematic::interpolatePath(PathKinematic_t path)
{
std::vector<StateKinematic_t> newStates;
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-20 20:29:42 UTC (rev 3346)
@@ -131,7 +131,7 @@
req.goal_state.vals[i] = 0.0;
req.goal_state.vals[0] = -1.0;
- req.allowed_time = 10.0;
+ req.allowed_time = 30.0;
req.volumeMin.x = -5.0 + m_basePos[0]; req.volumeMin.y = -5.0 + m_basePos[1]; req.volumeMin.z = 0.0;
req.volumeMax.x = 5.0 + m_basePos[0]; req.volumeMax.y = 5.0 + m_basePos[1]; req.volumeMax.z = 0.0;
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2008-08-20 20:29:42 UTC (rev 3346)
@@ -37,6 +37,8 @@
#include <urdf/URDF.h>
#include <libTF/Pose3D.h>
+
+#include <iostream>
#include <vector>
#include <string>
#include <cstdio>
@@ -47,91 +49,222 @@
namespace planning_models
{
-
+
+ /** Definition of a kinematic model */
class KinematicModel
{
public:
+ /** A basic definition of a shape. Shapes to be centered at origin */
+ class Shape
+ {
+ public:
+ Shape(void)
+ {
+ type = UNKNOWN;
+ }
+
+ virtual ~Shape(void)
+ {
+ }
+
+ enum { UNKNOWN, SPHERE, CYLINDER, BOX }
+ type;
+
+ };
- /** Possible geometry elements. These are assumed to be centered at origin (similar to ODE) */
- struct Geometry
+ /** Definition of a sphere */
+ class Sphere : public Shape
{
- Geometry(void)
+ public:
+ Sphere(void) : Shape()
{
- type = UNKNOWN;
- size[0] = size[1] = size[2] = 0.0;
+ type = SPHERE;
+ radius = 0.0;
}
- enum
- {
- UNKNOWN, BOX, CYLINDER, SPHERE
- } type;
+ double radius;
+ };
+
+ /** Definition of a cylinder */
+ class Cylinder : public Shape
+ {
+ public:
+ Cylinder(void) : Shape()
+ {
+ type = CYLINDER;
+ length = radius = 0.0;
+ }
- /** box: x, y, z
- cylinder: length, radius
- sphere: radius */
- double size[3];
+ double length, radius;
};
+ /** Definition of a box */
+ class Box : public Shape
+ {
+ public:
+ Box(void) : Shape()
+ {
+ type = BOX;
+ size[0] = size[1] = size[2] = 0.0;
+ }
+
+ /** x, y, z */
+ double size[3];
+ };
+
+ /** Forward definition of a joint */
class Joint;
- class Link;
+ /** Forward definition of a link */
+ class Link;
+
+ /** Forward definition of a robot */
+ class Robot;
+
/** A joint from the robot. Contains the transform applied by the joint type */
class Joint
{
public:
-
Joint(void)
{
usedParams = 0;
before = after = NULL;
- axis[0] = axis[1] = axis[2] = 0.0;
- anchor[0] = anchor[1] = anchor[2] = 0.0;
- limit[0] = limit[1] = 0.0;
- robotRoot = false;
- type = UNKNOWN;
+ owner = NULL;
}
- ~Joint(void)
+ virtual ~Joint(void)
{
if (after)
delete after;
}
+
+
+ /** Name of the joint */
+ std::string name;
+ /** The model that owns this joint */
+ KinematicModel *owner;
+
/** the links that this joint connects */
- Link *before;
- Link *after;
+ Link *before;
+ Link *after;
- /** Flag for marking joints that are roots for the entire robot */
- bool robotRoot;
-
/** The range of indices in the parameter vector that
needed to access information about the position of this
joint */
unsigned int usedParams;
/** Bitvector identifying which groups this joint is part of */
- std::vector<bool> inGroup;
+ std::vector<bool> inGroup;
- /* relevant joint information */
- enum
- {
- UNKNOWN, FIXED, REVOLUTE, PRISMATIC, PLANAR, FLOATING
- } type;
- double axis[3];
- double anchor[3];
- double limit[2];
-
- /* ----------------- Computed data -------------------*/
-
/** the local transform (computed by forward kinematics) */
- libTF::Pose3D varTrans;
- libTF::Pose3D globalTrans;
+ libTF::Pose3D varTrans;
+ /** Update varTrans if this joint is part of the group indicated by groupID
+ * and recompute globalTrans using varTrans */
const double* computeTransform(const double *params, int groupID = -1);
+ /** Update the value of VarTrans using the information from params */
+ virtual void updateVariableTransform(const double *params) = 0;
+
+ /** Extract the information needed by the joint given the URDF description */
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot) = 0;
};
+
+ /** A fixed joint */
+ class FixedJoint : public Joint
+ {
+ public:
+
+ /** Update the value of varTrans using the information from params */
+ virtual void updateVariableTransform(const double *params);
+
+ /** Extract the information needed by the joint given the URDF description */
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ };
+
+ /** A planar joint */
+ class PlanarJoint : public Joint
+ {
+ public:
+
+ PlanarJoint(void) : Joint()
+ {
+ usedParams = 3;
+ }
+
+ /** Update the value of varTrans using the information from params */
+ virtual void updateVariableTransform(const double *params);
+
+ /** Extract the information needed by the joint given the URDF description */
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ };
+
+ /** A floating joint */
+ class FloatingJoint : public Joint
+ {
+ public:
+
+ FloatingJoint(void) : Joint()
+ {
+ usedParams = 7;
+ }
+
+ /** Update the value of varTrans using the information from params */
+ virtual void updateVariableTransform(const double *params);
+
+ /** Extract the information needed by the joint given the URDF description */
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+ };
+
+ /** A prismatic joint */
+ class PrismaticJoint : public Joint
+ {
+ public:
+
+ PrismaticJoint(void) : Joint()
+ {
+ axis[0] = axis[1] = axis[2] = 0.0;
+ limit[0] = limit[1] = 0.0;
+ usedParams = 1;
+ }
+
+ /** Update the value of varTrans using the information from params */
+ virtual void updateVariableTransform(const double *params);
+
+ /** Extract the information needed by the joint given the URDF description */
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+
+ double axis[3];
+ double limit[2];
+ };
+ /** A revolute joint */
+ class RevoluteJoint : public Joint
+ {
+ public:
+
+ RevoluteJoint(void) : Joint()
+ {
+ axis[0] = axis[1] = axis[2] = 0.0;
+ anchor[0] = anchor[1] = anchor[2] = 0.0;
+ limit[0] = limit[1] = 0.0;
+ usedParams = 1;
+ }
+
+ /** Update the value of varTrans using the information from params */
+ virtual void updateVariableTransform(const double *params);
+
+ /** Extract the information needed by the joint given the URDF description */
+ virtual void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
+
+ double axis[3];
+ double anchor[3];
+ double limit[2];
+ };
+
+
/** A link from the robot. Contains the constant transform applied to the link and its geometry */
class Link
{
@@ -140,20 +273,24 @@
Link(void)
{
before = NULL;
- geom = new Geometry();
+ shape = NULL;
+ owner = NULL;
}
- ~Link(void)
+ virtual ~Link(void)
{
- if (geom)
- delete geom;
+ if (shape)
+ delete shape;
for (unsigned int i = 0 ; i < after.size() ; ++i)
delete after[i];
}
/** Name of the link */
std::string name;
-
+
+ /** The model that owns this link */
+ KinematicModel *owner;
+
/** Joint that connects this link to the parent link */
Joint *before;
@@ -167,14 +304,22 @@
libTF::Pose3D constGeomTrans;
/** The geometry of the link */
- Geometry *geom;
+ Shape *shape;
/* ----------------- Computed data -------------------*/
+ /** The global transform this link forwards (computed by forward kinematics) */
+ libTF::Pose3D globalTransFwd;
+
/** The global transform for this link (computed by forward kinematics) */
libTF::Pose3D globalTrans;
+
+ /** recompute globalTrans */
+ const double* computeTransform(const double *params, int groupID = -1);
+
+ /** Extract the information needed by the joint given the URDF description */
+ void extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot);
- const double* computeTransform(const double *params, int groupID = -1);
};
/** A robot structure */
@@ -197,6 +342,12 @@
void computeTransforms(const double *params, int groupID = -1);
+ /** The name of the robot */
+ std::string name;
+
+ /** The model that owns this robot */
+ KinematicModel *owner;
+
/** List of links in the robot */
std::vector<Link*> links;
@@ -204,8 +355,8 @@
std::vector<Link*> leafs;
/** The first joint in the robot -- the root */
- Joint *chain;
-
+ Joint *chain;
+
/** Number of parameters needed to define the joints */
unsigned int stateDimension;
@@ -235,16 +386,14 @@
/* The roots of every group within this robot */
std::vector<std::vector<Joint*> > groupChainStart;
-
- /** The model that owns this robot */
- KinematicModel *owner;
};
KinematicModel(void)
{
stateDimension = 0;
- m_verbose = false;
+ m_ignoreSensors = false;
+ m_verbose = false;
m_built = false;
}
@@ -254,7 +403,7 @@
delete m_robots[i];
}
- virtual void build(robot_desc::URDF &model);
+ virtual void build(robot_desc::URDF &model, bool ignoreSensors = false);
void setVerbose(bool verbose);
unsigned int getRobotCount(void) const;
@@ -262,9 +411,11 @@
void getGroups(std::vector<std::string> &groups) const;
int getGroupID(const std::string &group) const;
Link* getLink(const std::string &link) const;
+ void getLinks(std::vector<Link*> &links) const;
void computeTransforms(const double *params, int groupID = -1);
- void printModelInfo(FILE *out = stdout) const;
+ void printModelInfo(std::ostream &out = std::cout) const;
+ void printLinkPoses(std::ostream &out = std::cout) const;
/** A transform that is applied to the entire model */
libTF::Pose3D rootTransform;
@@ -293,6 +444,7 @@
std::vector<std::string> m_groups;
std::map<std::string, int> m_groupsMap;
std::map<std::string, Link*> m_linkMap;
+ bool m_ignoreSensors;
bool m_verbose;
bool m_built;
@@ -307,6 +459,9 @@
/** Construct the list of groups the model knows about (the ones marked with the 'plan' attribute) */
void constructGroupList(robot_desc::URDF &model);
+ /** Allocate a joint of appropriate type, depending on the loaded link */
+ Joint* createJoint(robot_desc::URDF::Link* urdfLink);
+
};
}
Modified: pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp 2008-08-20 20:29:42 UTC (rev 3346)
@@ -33,79 +33,203 @@
*********************************************************************/
#include <planning_models/kinematic.h>
+#include <algorithm>
#include <cmath>
+/** Operator for sorting objects by name */
+template<typename T>
+struct SortByName
+{
+ bool operator()(const T *a, const T *b) const
+ {
+ return a->name < b->name;
+ }
+};
+
void planning_models::KinematicModel::Robot::computeTransforms(const double *params, int groupID)
{
- if (chain->robotRoot)
- chain->globalTrans = owner->rootTransform;
chain->computeTransform(params, groupID);
}
void planning_models::KinematicModel::computeTransforms(const double *params, int groupID)
{
- unsigned int n = groupID >= 0 ? groupChainStart[groupID].size() : m_robots.size();
- for (unsigned int i = 0 ; i < n ; ++i)
+ if (groupID >= 0)
{
- Joint *start = groupID >= 0 ? groupChainStart[groupID][i] : m_robots[i]->chain;
- if (start->robotRoot)
- start->globalTrans = rootTransform;
- params = start->computeTransform(params, groupID);
- }
+ for (unsigned int i = 0 ; i < groupChainStart[groupID].size() ; ++i)
+ {
+ Joint *start = groupChainStart[groupID][i];
+ params = start->computeTransform(params, groupID);
+ }
+ }
+ else
+ {
+ for (unsigned int i = 0 ; i < m_robots.size(); ++i)
+ {
+ Joint *start = m_robots[i]->chain;
+ params = start->computeTransform(params, groupID);
+ }
+ }
}
-// we can optimize things here... (when we use identity transforms, for example)
const double* planning_models::KinematicModel::Joint::computeTransform(const double *params, int groupID)
{
unsigned int used = 0;
if (groupID < 0 || inGroup[groupID])
{
- switch (type)
- {
- case Joint::REVOLUTE:
- varTrans.setAxisAngle(axis, params[0]);
- break;
- case Joint::PRISMATIC:
- {
- double p = params[0];
- double dx = axis[0] * p;
- double dy = axis[1] * p;
- double dz = axis[2] * p;
- varTrans.setPosition(dx, dy, dz);
- }
- break;
- case Joint::PLANAR:
- varTrans.setPosition(params[0], params[1], 0.0);
- varTrans.setAxisAngle(0.0, 0.0, 1.0, params[2]);
- break;
- case Joint::FLOATING:
- varTrans.setPosition(params[0], params[1], params[2]);
- varTrans.setQuaternion(params[3], params[4], params[5], params[6]);
- break;
- default:
- break;
- }
+ updateVariableTransform(params);
used = usedParams;
}
- if (before)
- globalTrans = before->globalTrans; // otherwise, the caller initialized globalTrans already
- globalTrans.multiplyPose(varTrans);
return after->computeTransform(params + used, groupID);
}
const double* planning_models::KinematicModel::Link::computeTransform(const double *params, int groupID)
{
- const double *rparams = params;
- globalTrans = before->globalTrans;
- globalTrans.multiplyPose(constTrans);
+ globalTransFwd = before->before ? before->before->globalTransFwd : owner->rootTransform;
+ globalTransFwd.multiplyPose(constTrans);
+ globalTransFwd.multiplyPose(before->varTrans);
+
for (unsigned int i = 0 ; i < after.size() ; ++i)
- rparams = after[i]->computeTransform(rparams, groupID);
+ params = after[i]->computeTransform(params, groupID);
+
+ globalTrans = globalTransFwd;
globalTrans.multiplyPose(constGeomTrans);
- return rparams;
+
+ return params;
}
+void planning_models::KinematicModel::FixedJoint::updateVariableTransform(const double *params)
+{
+ // the joint remains identity
+}
+
+void planning_models::KinematicModel::FixedJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+{
+ // we need no data
+}
+
+void planning_models::KinematicModel::PlanarJoint::updateVariableTransform(const double *params)
+{
+ varTrans.setPosition(params[0], params[1], 0.0);
+ varTrans.setAxisAngle(0.0, 0.0, 1.0, params[2]);
+}
+
+void planning_models::KinematicModel::PlanarJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+{
+ robot->stateBounds.insert(robot->stateBounds.end(), 4, 0.0);
+ robot->stateBounds.push_back(-M_PI);
+ robot->stateBounds.push_back(M_PI);
+ robot->planarJoints.push_back(robot->stateDimension);
+}
+
+void planning_models::KinematicModel::FloatingJoint::updateVariableTransform(const double *params)
+{
+ varTrans.setPosition(params[0], params[1], params[2]);
+ varTrans.setQuaternion(params[3], params[4], params[5], params[6]);
+}
+
+void planning_models::KinematicModel::FloatingJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+{
+ robot->stateBounds.insert(robot->stateBounds.end(), 14, 0.0);
+ robot->floatingJoints.push_back(robot->stateDimension);
+}
+
+void planning_models::KinematicModel::PrismaticJoint::updateVariableTransform(const double *params)
+{
+ double p = params[0];
+ double dx = axis[0] * p;
+ double dy = axis[1] * p;
+ double dz = axis[2] * p;
+ varTrans.setPosition(dx, dy, dz);
+}
+
+void planning_models::KinematicModel::PrismaticJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+{
+ axis[0] = urdfLink->joint->axis[0];
+ axis[1] = urdfLink->joint->axis[1];
+ axis[2] = urdfLink->joint->axis[2];
+ limit[0] = urdfLink->joint->limit[0];
+ limit[1] = urdfLink->joint->limit[1];
+
+ robot->stateBounds.push_back(limit[0]);
+ robot->stateBounds.push_back(limit[1]);
+}
+
+void planning_models::KinematicModel::RevoluteJoint::updateVariableTransform(const double *params)
+{
+ varTrans.setAxisAngle(axis, params[0]);
+}
+
+void planning_models::KinematicModel::RevoluteJoint::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+{
+ axis[0] = urdfLink->joint->axis[0];
+ axis[1] = urdfLink->joint->axis[1];
+ axis[2] = urdfLink->joint->axis[2];
+ anchor[0] = urdfLink->joint->anchor[0];
+ anchor[1] = urdfLink->joint->anchor[1];
+ anchor[2] = urdfLink->joint->anchor[2];
+
+ // if the joint is continuous, we set the limit to [-Pi, Pi]
+ if (urdfLink->joint->isSet["limit"])
+ {
+ limit[0] = urdfLink->joint->limit[0];
+ limit[1] = urdfLink->joint->limit[1];
+ }
+ else
+ {
+ limit[0] = -M_PI;
+ limit[1] = M_PI;
+ }
+
+ robot->stateBounds.push_back(limit[0]);
+ robot->stateBounds.push_back(limit[1]);
+}
+
+void planning_models::KinematicModel::Link::extractInformation(const robot_desc::URDF::Link *urdfLink, Robot *robot)
+{
+ /* compute the geometry for this link */
+ switch (urdfLink->collision->geometry->type)
+ {
+ case robot_desc::URDF::Link::Geometry::BOX:
+ {
+ Box *box = new Box();
+ const double *size = static_cast<robot_desc::URDF::Link::Geometry::Box*>(urdfLink->collision->geometry->shape)->size;
+ box->size[0] = size[0];
+ box->size[1] = size[1];
+ box->size[2] = size[2];
+ shape = box;
+ }
+ break;
+ case robot_desc::URDF::Link::Geometry::SPHERE:
+ {
+ Sphere *sphere = new Sphere();
+ sphere->radius = static_cast<robot_desc::URDF::Link::Geometry::Sphere*>(urdfLink->collision->geometry->shape)->radius;
+ shape = sphere;
+ }
+ break;
+ case robot_desc::URDF::Link::Geometry::CYLINDER:
+ {
+ Cylinder *cylinder = new Cylinder();
+ cylinder->length = static_cast<robot_desc::URDF::Link::Geometry::Cylinder*>(urdfLink->collision->geometry->shape)->length;
+ cylinder->radius = static_cast<robot_desc::URDF::Link::Geometry::Cylinder*>(urdfLink->collision->geometry->shape)->radius;
+ shape = cylinder;
+ }
+ break;
+ default:
+ break;
+ }
+
+ /* compute the constant transform for this link */
+ const double *xyz = urdfLink->xyz;
+ const double *rpy = urdfLink->rpy;
+ constTrans.setFromEuler(xyz[0], xyz[1], xyz[2], rpy[2], rpy[1], rpy[0]);
+
+ xyz = urdfLink->collision->xyz;
+ rpy = urdfLink->collision->rpy;
+ constGeomTrans.setFromEuler(xyz[0], xyz[1], xyz[2], rpy[2], rpy[1], rpy[0]);
+}
+
void planning_models::KinematicModel::setVerbose(bool verbose)
{
m_verbose = verbose;
@@ -125,16 +249,17 @@
m_groupsMap[m_groups[i]] = i;
}
-void planning_models::KinematicModel::build(robot_desc::URDF &model)
+void planning_models::KinematicModel::build(robot_desc::URDF &model, bool ignoreSensors)
{
if (m_built)
{
- fprintf(stderr, "Model has already been built!\n");
+ std::cerr << "Model has already been built!" << std::endl;
return;
}
m_built = true;
-
+ m_ignoreSensors = ignoreSensors;
+
/* construct a map for the available groups */
constructGroupList(model);
groupStateIndexList.resize(m_groups.size());
@@ -143,10 +268,13 @@
for (unsigned int i = 0 ; i < model.getDisjointPartCount() ; ++i)
{
robot_desc::URDF::Link *link = model.getDisjointPart(i);
- Robot *rb = new Robot(this);
+ if (link->canSense() && m_ignoreSensors)
+ continue;
+ Robot *rb = new Robot(this);
+ rb->name = model.getRobotName();
rb->groupStateIndexList.resize(m_groups.size());
rb->groupChainStart.resize(m_groups.size());
- rb->chain = new Joint();
+ rb->chain = createJoint(link);
buildChainJ(rb, NULL, rb->chain, link, model);
m_robots.push_back(rb);
}
@@ -201,6 +329,15 @@
return m_robots[index];
}
+void planning_models::KinematicModel::getLinks(std::vector<Link*> &links) const
+{
+ std::vector<Link*> localLinks;
+ for (std::map<std::string, Link*>::const_iterator it = m_linkMap.begin() ; it != m_linkMap.end() ; ++it)
+ localLinks.push_back(it->second);
+ std::sort(localLinks.begin(), localLinks.end(), SortByName<Link>());
+ links.insert(links.end(), localLinks.begin(), localLinks.end());
+}
+
planning_models::KinematicModel::Link* planning_models::KinematicModel::getLink(const std::string &link) const
{
std::map<std::string, Link*>::const_iterator pos = m_linkMap.find(link);
@@ -211,63 +348,11 @@
{
joint->before = parent;
joint->after = new Link();
+ joint->name = urdfLink->joint->name;
+ joint->owner = robot->owner;
- if (model.isRoot(urdfLink))
- joint->robotRoot = true;
+ joint->extractInformation(urdfLink, robot);
- /* copy relevant data */
- joint->limit[0] = urdfLink->joint->limit[0];
- joint->limit[1] = urdfLink->joint->limit[1];
- joint->axis[0] = urdfLink->joint->axis[0];
- joint->axis[1] = urdfLink->joint->axis[1];
- joint->axis[2] = urdfLink->joint->axis[2];
- joint->anchor[0] = urdfLink->joint->anchor[0];
- joint->anchor[1] = urdfLink->joint->anchor[1];
- joint->anchor[2] = urdfLink->joint->anchor[2];
- if (!urdfLink->joint->isSet["limit"] && urdfLink->joint->type == robot_desc::URDF::Link::Joint::REVOLUTE)
- {
- joint->limit[0] = -M_PI;
- joint->limit[1] = M_PI;
- }
-
- switch (urdfLink->joint->type)
- {
- case robot_desc::URDF::Link::Joint::FLOATING:
- joint->type = Joint::FLOATING;
- joint->usedParams = 7;
- robot->stateBounds.insert(robot->stateBounds.end(), 14, 0.0);
- robot->floatingJoints.push_back(robot->stateDimension);
- break;
- case robot_desc::URDF::Link::Joint::PLANAR:
- joint->type = Joint::PLANAR;
- joint->usedParams = 3;
- robot->stateBounds.insert(robot->stateBounds.end(), 4, 0.0);
- robot->stateBounds.push_back(-M_PI);
- robot->stateBounds.push_back(M_PI);
- robot->planarJoints.push_back(robot->stateDimension);
- break;
- case robot_desc::URDF::Link::Joint::FIXED:
- joint->type = Joint::FIXED;
- joint->usedParams = 0;
- break;
- case robot_desc::URDF::Link::Joint::REVOLUTE:
- joint->type = Joint::REVOLUTE;
- joint->usedParams = 1;
- robot->stateBounds.push_back(joint->limit[0]);
- robot->stateBounds.push_back(joint->limit[1]);
- break;
- case robot_desc::URDF::Link::Joint::PRISMATIC:
- joint->type = Joint::PRISMATIC;
- joint->usedParams = 1;
- robot->stateBounds.push_back(joint->limit[0]);
- robot->stateBounds.push_back(joint->limit[1]);
- break;
- default:
- joint->type = Joint::UNKNOWN;
- joint->usedParams = 0;
- break;
- }
-
/** construct the inGroup bitvector */
std::vector<std::string> gnames;
model.getGroupNames(gnames);
@@ -283,18 +368,17 @@
for (unsigned int k = 0 ; k < urdfLink->groups.size() ; ++k)
if (urdfLink->groups[k]->isRoot(urdfLink))
{
- std::string gname = model.getRobotName() + "::" + urdfLink->groups[k]->name;
+ std::string gname = robot->name + "::" + urdfLink->groups[k]->name;
if (m_groupsMap.find(gname) != m_groupsMap.end())
robot->groupChainStart[m_groupsMap[gname]].push_back(joint);
}
if (m_verbose && joint->usedParams > 0)
{
- printf("Joint '%s' connects link '%s' to link '%s' and uses state coordinates: ",
- urdfLink->joint->name.c_str(), urdfLink->parentName.c_str(), urdfLink->name.c_str());
+ std::cout << "Joint '" << urdfLink->joint->name << "' connects link '" << urdfLink->parentName << "' to link '" << urdfLink->name << "' and uses state coordinates: ";
for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
- printf("%d ", i + robot->stateDimension);
- printf("\n");
+ std::cout << i + robot->stateDimension << " ";
+ std::cout << std::endl;
}
robot->stateDimension += joint->usedParams;
@@ -303,92 +387,98 @@
void planning_models::KinematicModel::buildChainL(Robot *robot, Joint *parent, Link* link, robot_desc::URDF::Link* urdfLink, robot_desc::URDF &model)
{
- link->name = urdfLink->name;
+ link->name = urdfLink->name;
link->before = parent;
+ link->owner = robot->owner;
robot->links.push_back(link);
- /* copy relevant data */
- switch (urdfLink->collision->geometry->type)
- {
- case robot_desc::URDF::Link::Geometry::UNKNOWN:
- link->geom->type = Geometry::UNKNOWN;
- break;
- case robot_desc::URDF::Link::Geometry::BOX:
- link->geom->type = Geometry::BOX;
- {
- const double *size = static_cast<robot_desc::URDF::Link::Geometry::Box*>(urdfLink->collision->geometry->shape)->size;
- link->geom->size[0] = size[0];
- link->geom->size[1] = size[1];
- link->geom->size[2] = size[2];
- }
- break;
- case robot_desc::URDF::Link::Geometry::SPHERE:
- link->geom->type = Geometry::SPHERE;
- link->geom->size[0] = static_cast<robot_desc::URDF::Link::Geometry::Sphere*>(urdfLink->collision->geometry->shape)->radius;
- break;
- case robot_desc::URDF::Link::Geometry::CYLINDER:
- link->geom->type = Geometry::CYLINDER;
- link->geom->size[0] = static_cast<robot_desc::URDF::Link::Geometry::Cylinder*>(urdfLink->collision->geometry->shape)->radius;
- link->geom->size[1] = static_cast<robot_desc::URDF::Link::Geometry::Cylinder*>(urdfLink->collision->geometry->shape)->length;
- break;
- default:
- break;
- }
+ link->extractInformation(urdfLink, robot);
- /* compute the constant transform for this link */
- double *xyz = urdfLink->xyz;
- double *rpy = urdfLink->rpy;
- link->constTrans.setFromEuler(xyz[0], xyz[1], xyz[2], rpy[2], rpy[1], rpy[0]);
-
- xyz = urdfLink->collision->xyz;
- rpy = urdfLink->collision->rpy;
- link->constGeomTrans.setFromEuler(xyz[0], xyz[1], xyz[2], rpy[2], rpy[1], rpy[0]);
-
for (unsigned int i = 0 ; i < urdfLink->children.size() ; ++i)
{
- Joint *newJoint = new Joint();
+ if (urdfLink->children[i]->canSense() && m_ignoreSensors)
+ continue;
+ Joint *newJoint = createJoint(urdfLink);
buildChainJ(robot, link, newJoint, urdfLink->children[i], model);
link->after.push_back(newJoint);
}
-
+
if (link->after.size() == 0)
robot->leafs.push_back(link);
}
-void planning_models::KinematicModel::printModelInfo(FILE *out) const
+planning_models::KinematicModel::Joint* planning_models::KinematicModel::createJoint(robot_desc::URDF::Link* urdfLink)
+{
+ Joint *newJoint = NULL;
+ switch (urdfLink->joint->type)
+ {
+ case robot_desc::URDF::Link::Joint::FIXED:
+ newJoint = new FixedJoint();
+ break;
+ case robot_desc::URDF::Link::Joint::FLOATING:
+ newJoint = new FloatingJoint();
+ break;
+ case robot_desc::URDF::Link::Joint::PLANAR:
+ newJoint = new PlanarJoint();
+ break;
+ case robot_desc::URDF::Link::Joint::PRISMATIC:
+ newJoint = new PrismaticJoint();
+ break;
+ case robot_desc::URDF::Link::Joint::REVOLUTE:
+ newJoint = new RevoluteJoint();
+ break;
+ default:
+ std::cerr << "Unknown joint type " << urdfLink->joint->type << std::endl;
+ break;
+ }
+ return newJoint;
+}
+
+void planning_models::KinematicModel::printModelInfo(std::ostream &out) const
{
- fprintf(out, "Number of robots = %d\n", getRobotCount());
- fprintf(out, "Complete model state dimension = %d\n", stateDimension);
-
- fprintf(out, "State bounds: ");
+ out << "Number of robots = " << getRobotCount() << std::endl;
+ out << "Complete model state dimension = " << stateDimension << std::endl;
+
+ out << "State bounds: ";
for (unsigned int i = 0 ; i < stateDimension ; ++i)
- fprintf(out, "[%f, %f] ", stateBounds[2 * i], stateBounds[2 * i + 1]);
- fprintf(out, "\n");
+ out << "[" << stateBounds[2 * i] << ", " << stateBounds[2 * i + 1] << "] ";
+ out << std::endl;
- fprintf(out, "Floating joints at: ");
+ out << "Floating joints at: ";
for (unsigned int i = 0 ; i < floatingJoints.size() ; ++i)
- fprintf(out, "%d ", floatingJoints[i]);
- fprintf(out, "\n");
-
- fprintf(out, "Planar joints at: ");
+ out << floatingJoints[i] << " ";
+ out << std::endl;
+
+ out << "Planar joints at: ";
for (unsigned int i = 0 ; i < planarJoints.size() ; ++i)
- fprintf(out, "%d ", planarJoints[i]);
- fprintf(out, "\n");
-
- fprintf(out, "Available groups: ");
- std::vector<std::string> l;
+ out << planarJoints[i] << " ";
+ out << std::endl;
+
+ out << "Available groups: ";
+ std::vector<std::string> l;
getGroups(l);
for (unsigned int i = 0 ; i < l.size() ; ++i)
- fprintf(out, "%s ", l[i].c_str());
- fprintf(out, "\n");
+ out << l[i] << " ";
+ out << std::endl;
for (unsigned int i = 0 ; i < l.size() ; ++i)
{
int gid = getGroupID(l[i]);
- fprintf(out, "Group %s has %d roots\n", l[i].c_str(), groupChainStart[gid].size());
- fprintf(out, "The state components for this group are: ");
+ out << "Group " << l[i] << " has " << groupChainStart[gid].size() << " roots" << std::endl;
+ out << "The state components for this group are: ";
for (unsigned int j = 0 ; j < groupStateIndexList[gid].size() ; ++j)
- fprintf(out, "%d ", groupStateIndexList[gid][j]);
- fprintf(out, "\n");
- }
+ out << groupStateIndexList[gid][j] << " ";
+ out << std::endl;
+ }
}
+
+void planning_models::KinematicModel::printLinkPoses(std::ostream &out) const
+{
+ std::vector<Link*> links;
+ getLinks(links);
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ {
+ out << links[i]->name << std::endl;
+ out << links[i]->globalTrans << std::endl;
+ }
+}
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-20 20:29:31 UTC (rev 3345)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-20 20:29:42 UTC (rev 3346)
@@ -38,8 +38,8 @@
@htmlinclude ../../manifest.html
-@b NodeODECollisionModel is a class which in addition to being aware of a robot model,
-is also aware of an ODE collision space.
+@b NodeCollisionModel is a class which in addition to being aware of a robot model,
+is also aware of a collision space.
<hr>
@@ -74,31 +74,37 @@
#include <planning_node_util/knode.h>
#include <std_msgs/PointCloudFloat32.h>
#include <collision_space/environmentODE.h>
+#include <collision_space/environmentOctree.h>
name...
[truncated message content] |
