Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8083] pkg/trunk
|
From: <is...@us...> - 2008-12-15 02:19:34
|
Revision: 8083
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8083&view=rev
Author: isucan
Date: 2008-12-15 02:19:31 +0000 (Mon, 15 Dec 2008)
Log Message:
-----------
ported motion planning code to using bullet instead of libTF
Modified Paths:
--------------
pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h
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/kinematic_planning/include/RKPBasicRequest.h
pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml
pkg/trunk/motion_planning/planning_node_util/manifest.xml
pkg/trunk/visualization/ogre_visualizer/manifest.xml
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp
pkg/trunk/world_models/world_3d_map/manifest.xml
Removed Paths:
-------------
pkg/trunk/deprecated/planning_models_with_libTF/
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/include/collision_space/environment.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h 2008-12-15 02:19:31 UTC (rev 8083)
@@ -38,7 +38,6 @@
#define COLLISION_SPACE_ENVIRONMENT_MODEL_
#include <planning_models/kinematic.h>
-#include <profiling_utils/profiler.h>
#include <rosthread/mutex.h>
#include <vector>
#include <string>
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-12-15 02:19:31 UTC (rev 8083)
@@ -40,9 +40,6 @@
#include <collision_space/environment.h>
#include <ode/ode.h>
-// check() is apparently defined as a macro on OS X (?)
-#undef check
-
/** @htmlinclude ../../manifest.html
A class describing an environment for a kinematic robot using ODE */
@@ -195,8 +192,8 @@
std::vector<Geom*> m_geomsY;
std::vector<Geom*> m_geomsZ;
- void check(std::vector<Geom*>::iterator posStart, std::vector<Geom*>::iterator posEnd,
- Geom *g, void *data, dNearCallback *nearCallback);
+ void checkColl(std::vector<Geom*>::iterator posStart, std::vector<Geom*>::iterator posEnd,
+ Geom *g, void *data, dNearCallback *nearCallback);
};
struct kGeom
Deleted: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-12-15 02:19:31 UTC (rev 8083)
@@ -1,100 +0,0 @@
-/*********************************************************************
-* 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, Matei Ciocarlie */
-
-#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, 0.02f, 0.02f, 0.02f, 1, OCTREE_CELL_EMPTY)
- {
- m_octree.setAutoExpand(true);
- m_selfCollision = false;
- }
-
- virtual ~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 plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
- virtual void addStaticPlane(double a, double b, double c, double d);
-
- /** Add a robot model. Ignore robot links if their name is not specified in the string vector */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links);
-
- /** Update the positions of the geometry used in collision detection */
- virtual void updateRobotModel(unsigned int model_id);
-
- /** Add a group of links to be checked for self collision */
- virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links);
-
- 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-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/util.h 2008-12-15 02:19:31 UTC (rev 8083)
@@ -37,7 +37,7 @@
#ifndef COLLISION_SPACE_UTIL_
#define COLLISION_SPACE_UTIL_
-#include <libTF/Pose3D.h>
+#include <LinearMath/btTransform.h>
#include <cmath>
/**
@@ -72,7 +72,7 @@
updateInternalData();
}
- void setPose(const libTF::Pose3D &pose)
+ void setPose(const btTransform &pose)
{
m_pose = pose;
updateInternalData();
@@ -86,20 +86,18 @@
bool containsPoint(double x, double y, double z) const
{
- libTF::Position pt(x, y, z);
- return containsPoint(pt);
+ return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
}
- virtual bool containsPoint(const libTF::Position &p) const = 0;
+ virtual bool containsPoint(const btVector3 &p) const = 0;
protected:
virtual void updateInternalData(void) = 0;
virtual void useDimensions(const double *dims) = 0;
- libTF::Pose3D m_pose;
- double m_scale;
-
+ btTransform m_pose;
+ double m_scale;
};
class Sphere : public Shape
@@ -114,12 +112,9 @@
{
}
- virtual bool containsPoint(const libTF::Position &p) const
+ virtual bool containsPoint(const btVector3 &p) const
{
- double dx = m_center.x - p.x;
- double dy = m_center.y - p.y;
- double dz = m_center.z - p.z;
- return dx * dx + dy * dy + dz * dz < m_radius2;
+ return (m_center - p).length2() < m_radius2;
}
protected:
@@ -132,20 +127,18 @@
virtual void updateInternalData(void)
{
m_radius2 = m_radius * m_radius * m_scale * m_scale;
-
- m_pose.getPosition(m_center);
+ m_center = m_pose.getOrigin();
}
- libTF::Position m_center;
- double m_radius;
- double m_radius2;
-
+ btVector3 m_center;
+ double m_radius;
+ double m_radius2;
};
class Cylinder : public Shape
{
public:
- Cylinder(void) : Shape()
+ Cylinder(void) : Shape(), m_normalH(btScalar(0.0), btScalar(0.0), btScalar(1.0))
{
m_length = m_radius = 0.0;
}
@@ -154,19 +147,16 @@
{
}
- virtual bool containsPoint(const libTF::Position &p) const
+ virtual bool containsPoint(const btVector3 &p) const
{
- double vx = p.x - m_center.x;
- double vy = p.y - m_center.y;
- double vz = p.z - m_center.z;
+ btVector3 v = p - m_center;
+ double pH = v.dot(m_normalH);
- double pH = vx * m_normalH.x + vy * m_normalH.y + vz * m_normalH.z;
-
if (fabs(pH) > m_length2)
return false;
- double pB1 = vx * m_normalB1.x + vy * m_normalB1.y + vz * m_normalB1.z;
- double pB2 = vx * m_normalB2.x + vy * m_normalB2.y + vz * m_normalB2.z;
+ double pB1 = v.dot(m_normalB1);
+ double pB2 = v.dot(m_normalB2);
return pB1 * pB2 < m_radius2;
}
@@ -182,31 +172,28 @@
virtual void updateInternalData(void)
{
m_radius2 = m_radius * m_radius * m_scale * m_scale;
- m_length2 = m_scale * m_length / 2.0;
+ m_length2 = m_scale * m_length / 2.0;
+ m_center = m_pose.getOrigin();
- m_pose.getPosition(m_center);
+ m_normalH.setValue(btScalar(0.0), btScalar(0.0), btScalar(1.0));
+ m_normalH = m_pose * m_normalH;
- // this can probably be optimized by simply taking
- // the columns of the transform matrix
- m_normalH.x = m_normalH.y = 0.0; m_normalH.z = 1.0;
- m_pose.applyToVector(m_normalH);
-
- m_normalB1.y = m_normalB1.z = 0.0; m_normalB1.x = 1.0;
- m_pose.applyToVector(m_normalB1);
-
- m_normalB2.x = m_normalB2.z = 0.0; m_normalB2.y = 1.0;
- m_pose.applyToVector(m_normalB2);
+ m_normalB1.setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0));
+ m_normalB1 = m_pose * m_normalB1;
+
+ m_normalB2.setValue(btScalar(0.0), btScalar(1.0), btScalar(0.0));
+ m_normalB2 = m_pose * m_normalB2;
}
- libTF::Position m_center;
- libTF::Vector m_normalH;
- libTF::Vector m_normalB1;
- libTF::Vector m_normalB2;
+ btVector3 m_center;
+ btVector3 m_normalH;
+ btVector3 m_normalB1;
+ btVector3 m_normalB2;
- double m_length;
- double m_length2;
- double m_radius;
- double m_radius2;
+ double m_length;
+ double m_length2;
+ double m_radius;
+ double m_radius2;
};
@@ -222,23 +209,20 @@
{
}
- virtual bool containsPoint(const libTF::Position &p) const
+ virtual bool containsPoint(const btVector3 &p) const
{
- double vx = p.x - m_center.x;
- double vy = p.y - m_center.y;
- double vz = p.z - m_center.z;
+ btVector3 v = p - m_center;
+ double pL = v.dot(m_normalL);
- double pL = vx * m_normalL.x + vy * m_normalL.y + vz * m_normalL.z;
-
if (fabs(pL) > m_length2)
return false;
- double pW = vx * m_normalW.x + vy * m_normalW.y + vz * m_normalW.z;
+ double pW = v.dot(m_normalW);
if (fabs(pW) > m_width2)
return false;
- double pH = vx * m_normalH.x + vy * m_normalH.y + vz * m_normalH.z;
+ double pH = v.dot(m_normalH);
if (fabs(pH) > m_height2)
return false;
@@ -261,33 +245,32 @@
m_width2 = m_scale * m_width / 2.0;
m_height2 = m_scale * m_height / 2.0;
- m_pose.getPosition(m_center);
+ m_center = m_pose.getOrigin();
- m_normalH.x = m_normalH.y = 0.0; m_normalH.z = 1.0;
- m_pose.applyToVector(m_normalH);
-
- m_normalL.y = m_normalL.z = 0.0; m_normalL.x = 1.0;
- m_pose.applyToVector(m_normalL);
-
- m_normalW.x = m_normalW.z = 0.0; m_normalW.y = 1.0;
- m_pose.applyToVector(m_normalW);
+ m_normalH.setValue(btScalar(0.0), btScalar(0.0), btScalar(1.0));
+ m_normalH = m_pose * m_normalH;
+
+ m_normalL.setValue(btScalar(1.0), btScalar(0.0), btScalar(0.0));
+ m_normalL = m_pose * m_normalL;
+
+ m_normalW.setValue(btScalar(0.0), btScalar(1.0), btScalar(0.0));
+ m_normalW = m_pose * m_normalW;
}
- libTF::Position m_center;
- libTF::Vector m_normalL;
- libTF::Vector m_normalW;
- libTF::Vector m_normalH;
+ btVector3 m_center;
+ btVector3 m_normalL;
+ btVector3 m_normalW;
+ btVector3 m_normalH;
- double m_length;
- double m_width;
- double m_height;
- double m_length2;
- double m_width2;
- double m_height2;
+ double m_length;
+ double m_width;
+ double m_height;
+ double m_length2;
+ double m_width2;
+ double m_height2;
};
}
}
#endif
-
Modified: pkg/trunk/motion_planning/collision_space/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/collision_space/manifest.xml 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/manifest.xml 2008-12-15 02:19:31 UTC (rev 8083)
@@ -8,19 +8,9 @@
<review status="unreviewed" notes=""/>
<depend package="rosthread"/>
- <depend package="planning_models_with_libTF"/>
-
-<!--
- We currently disable Octree collision checking since dependency on
- scan_utils means a lot of compile time and collision checking with
- octrees is not capable of certain operations. Octrees could be used
- in a combined collision checker (combined with ODE, for example).
-
- <depend package="scan_utils"/>
--->
-
+ <depend package="planning_models"/>
<depend package="opende"/>
- <depend package="profiling_utils"/>
+ <depend package="bullet"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lcollision_space"/>
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-12-15 02:19:31 UTC (rev 8083)
@@ -128,13 +128,14 @@
for (unsigned int i = 0 ; i < n ; ++i)
{
- libTF::Pose3D &pose = m_kgeoms[model_id].geom[i]->link->globalTrans;
- dGeomID geom = m_kgeoms[model_id].geom[i]->geom;
+ btTransform &pose = m_kgeoms[model_id].geom[i]->link->globalTrans;
+ dGeomID geom = m_kgeoms[model_id].geom[i]->geom;
- libTF::Position pos = pose.getPosition();
- dGeomSetPosition(geom, pos.x, pos.y, pos.z);
- libTF::Quaternion quat = pose.getQuaternion();
- dQuaternion q; q[0] = quat.w; q[1] = quat.x; q[2] = quat.y; q[3] = quat.z;
+ btVector3 pos = pose.getOrigin();
+ dGeomSetPosition(geom, pos.getX(), pos.getY(), pos.getZ());
+ btQuaternion quat = pose.getRotation();
+ dQuaternion q;
+ q[0] = quat.getW(); q[1] = quat.getX(); q[2] = quat.getY(); q[3] = quat.getZ();
dGeomSetQuaternion(geom, q);
}
}
@@ -178,8 +179,8 @@
}
}
-void collision_space::EnvironmentModelODE::ODECollide2::check(std::vector<Geom*>::iterator posStart, std::vector<Geom*>::iterator posEnd,
- Geom *g, void *data, dNearCallback *nearCallback)
+void collision_space::EnvironmentModelODE::ODECollide2::checkColl(std::vector<Geom*>::iterator posStart, std::vector<Geom*>::iterator posEnd,
+ Geom *g, void *data, dNearCallback *nearCallback)
{
/* posStart now identifies the first geom which has an AABB
that could overlap the AABB of geom on the X axis. posEnd
@@ -235,23 +236,23 @@
if (d3 > CUTOFF)
{
if (d3 <= d2 && d3 <= d1)
- check(posStart3, posEnd3, &g, data, nearCallback);
+ checkColl(posStart3, posEnd3, &g, data, nearCallback);
else
if (d2 <= d3 && d2 <= d1)
- check(posStart2, posEnd2, &g, data, nearCallback);
+ checkColl(posStart2, posEnd2, &g, data, nearCallback);
else
- check(posStart1, posEnd1, &g, data, nearCallback);
+ checkColl(posStart1, posEnd1, &g, data, nearCallback);
}
else
- check(posStart3, posEnd3, &g, data, nearCallback);
+ checkColl(posStart3, posEnd3, &g, data, nearCallback);
}
}
else
- check(posStart2, posEnd2, &g, data, nearCallback);
+ checkColl(posStart2, posEnd2, &g, data, nearCallback);
}
}
else
- check(posStart1, posEnd1, &g, data, nearCallback);
+ checkColl(posStart1, posEnd1, &g, data, nearCallback);
}
}
@@ -294,8 +295,6 @@
CollisionData cdata;
cdata.collides = false;
- profiling_utils::Profiler::Begin("Check self collision");
-
/* check self collision */
if (m_selfCollision)
{
@@ -332,10 +331,6 @@
/* check collision with standalone ode bodies */
OUT1:
- profiling_utils::Profiler::End("Check self collision");
-
- profiling_utils::Profiler::Begin("Check basic objects collision");
-
if (!cdata.collides)
{
for (int i = m_kgeoms[model_id].geom.size() - 1 ; i >= 0 ; --i)
@@ -364,9 +359,6 @@
/* check collision with pointclouds */
OUT2:
- profiling_utils::Profiler::End("Check basic objects collision");
-
- profiling_utils::Profiler::Begin("Check pointcloud collision");
if (!cdata.collides)
{
m_collide2.setup();
@@ -374,8 +366,6 @@
m_collide2.collide(m_kgeoms[model_id].geom[i]->geom, reinterpret_cast<void*>(&cdata), nearCallbackFn);
}
- profiling_utils::Profiler::End("Check pointcloud collision");
-
return cdata.collides;
}
Deleted: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-12-15 02:19:31 UTC (rev 8083)
@@ -1,159 +0,0 @@
-/*********************************************************************
-* 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, Matei Ciocarlie */
-
-#include <collision_space/environmentOctree.h>
-
-unsigned int collision_space::EnvironmentModelOctree::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links)
-{
- unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links);
- if (id <= m_modelLinks.size())
- m_modelLinks.resize(id + 1);
-
- std::vector< planning_models::KinematicModel::Link*> allLinks;
-
- std::map<std::string, bool> exists;
- for (unsigned int i = 0 ; i < links.size() ; ++i)
- exists[links[i]] = true;
-
- for (unsigned int i = 0 ; i < allLinks.size() ; ++i)
- if (exists.find(allLinks[i]->name) != exists.end())
- m_modelLinks[id].push_back(allLinks[i]);
-
- 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]->shape->type)
- {
- case planning_models::KinematicModel::Shape::BOX:
- {
- /** \todo math here is a bit clumsy.... should be fixed when bette libTF is available */
- 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::Position libTFpos;
- links[i]->globalTrans.getPosition(libTFpos);
- float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z};
- const double *size = static_cast<planning_models::KinematicModel::Box*>(links[i]->shape)->size;
- const float sizef[3] = {size[0], size[1], size[2]};
-
- result = m_octree.intersectsBox(pos, sizef, axes);
- }
-
- break;
-
- case planning_models::KinematicModel::Shape::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::Position libTFpos;
- links[i]->globalTrans.getPosition(libTFpos);
- float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z };
- float radius = static_cast<planning_models::KinematicModel::Cylinder*>(links[i]->shape)->radius;
- float length = static_cast<planning_models::KinematicModel::Cylinder*>(links[i]->shape)->length;
- float L = radius * 2.0;
- const float sizef[3] = {length, L, L};
-
- result = m_octree.intersectsBox(pos, sizef, axes);
- }
- break;
-
- case planning_models::KinematicModel::Shape::SPHERE:
- {
- libTF::Position libTFpos;
- links[i]->globalTrans.getPosition(libTFpos);
- float pos[3] = {libTFpos.x, libTFpos.y, libTFpos.z};
- float radius = static_cast<planning_models::KinematicModel::Sphere*>(links[i]->shape)->radius;
- result = m_octree.intersectsSphere(pos, radius);
- }
- break;
- default:
- fprintf(stderr, "Geometry type not implemented: %d\n", links[i]->shape->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;
-}
-
-void collision_space::EnvironmentModelOctree::addStaticPlane(double a, double b, double c, double d)
-{
- fprintf(stderr, "Octree collision checking does not support planes\n");
-}
-
-void collision_space::EnvironmentModelOctree::addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links)
-{
- fprintf(stderr, "Octree collision checking does not support self collision\n");
-}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPBasicRequest.h 2008-12-15 02:19:31 UTC (rev 8083)
@@ -269,11 +269,7 @@
double bestDifference = 0.0;
m->collisionSpace->lock();
- profiling_utils::Profiler::Start();
-
computePlan(psetup, req.times, req.allowed_time, req.interpolate, bestPath, bestDifference);
-
- profiling_utils::Profiler::Stop();
m->collisionSpace->unlock();
/* fill in the results */
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RKPConstraintEvaluator.h 2008-12-15 02:19:31 UTC (rev 8083)
@@ -105,33 +105,30 @@
void evaluate(double *distPos, double *distAng)
{
if (m_link)
- {
- switch (m_pc.type)
+ {
+ if (distPos)
{
- case robot_msgs::PoseConstraint::ONLY_POSITION:
- if (distPos)
+ if ((m_pc.type == robot_msgs::PoseConstraint::ONLY_POSITION || m_pc.type == robot_msgs::PoseConstraint::COMPLETE_POSE))
+ {
+ btVector3 bodyPos = m_link->globalTrans.getOrigin();
+ double dx = bodyPos.getX() - m_pc.pose.position.x;
+ double dy = bodyPos.getY() - m_pc.pose.position.y;
+ double dz = bodyPos.getZ() - m_pc.pose.position.z;
+ *distPos = dx * dx + dy * dy + dz * dz;
+ }
+ else
+ *distPos = 0.0;
+ }
+
+ if (distAng)
+ {
+ if ((m_pc.type == robot_msgs::PoseConstraint::ONLY_ORIENTATION || m_pc.type == robot_msgs::PoseConstraint::COMPLETE_POSE))
{
- libTF::Position bodyPos;
- m_link->globalTrans.getPosition(bodyPos);
-
- double dx = bodyPos.x - m_pc.pose.position.x;
- double dy = bodyPos.y - m_pc.pose.position.y;
- double dz = bodyPos.z - m_pc.pose.position.z;
-
- *distPos = dx * dx + dy * dy + dz * dz;
+ btQuaternion quat(m_pc.pose.orientation.x, m_pc.pose.orientation.y, m_pc.pose.orientation.z, m_pc.pose.orientation.w);
+ *distAng = quat.angle(m_link->globalTrans.getRotation());
}
- if (distAng)
+ else
*distAng = 0.0;
- break;
-
- case robot_msgs::PoseConstraint::ONLY_ORIENTATION:
- case robot_msgs::PoseConstraint::COMPLETE_POSE:
- default:
- if (distPos)
- *distPos = 0.0;
- if (distAng)
- *distAng = 0.0;
- break;
}
}
else
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-12-15 02:19:31 UTC (rev 8083)
@@ -466,7 +466,5 @@
else
usage(argv[0]);
- profiling_utils::Profiler::Status();
-
return 0;
}
Modified: pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/plan_kinematic_path/manifest.xml 2008-12-15 02:19:31 UTC (rev 8083)
@@ -7,6 +7,6 @@
<depend package="robot_msgs" />
<depend package="robot_srvs" />
<depend package="planning_node_util" />
- <depend package="planning_models_with_libTF" />
+ <depend package="planning_models" />
<depend package="pr2_mechanism_controllers" />
</package>
Modified: pkg/trunk/motion_planning/planning_node_util/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/manifest.xml 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/motion_planning/planning_node_util/manifest.xml 2008-12-15 02:19:31 UTC (rev 8083)
@@ -13,7 +13,7 @@
<depend package="robot_msgs"/>
<depend package="tf"/>
<depend package="wg_robot_description_parser"/>
- <depend package="planning_models_with_libTF"/>
+ <depend package="planning_models"/>
<depend package="collision_space"/>
<export>
Modified: pkg/trunk/visualization/ogre_visualizer/manifest.xml
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/manifest.xml 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/visualization/ogre_visualizer/manifest.xml 2008-12-15 02:19:31 UTC (rev 8083)
@@ -22,7 +22,7 @@
<depend package="scan_utils" />
<depend package="wxpropgrid"/>
<depend package="wx_topic_display"/>
- <depend package="planning_models_with_libTF"/>
+ <depend package="planning_models"/>
<depend package="robot_msgs" />
<depend package="wxswig"/>
<depend package="wxPython_swig_interface"/>
Modified: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/helpers/robot.cpp 2008-12-15 02:19:31 UTC (rev 8083)
@@ -666,17 +666,17 @@
continue;
}
- libTF::Position robot_visual_position = link->globalTransFwd.getPosition();
- libTF::Quaternion robot_visual_orientation = link->globalTransFwd.getQuaternion();
- Ogre::Vector3 visual_position( robot_visual_position.x, robot_visual_position.y, robot_visual_position.z );
- Ogre::Quaternion visual_orientation( robot_visual_orientation.w, robot_visual_orientation.x, robot_visual_orientation.y, robot_visual_orientation.z );
+ btVector3 robot_visual_position = link->globalTransFwd.getOrigin();
+ btQuaternion robot_visual_orientation = link->globalTransFwd.getRotation();
+ Ogre::Vector3 visual_position( robot_visual_position.getX(), robot_visual_position.getY(), robot_visual_position.getZ() );
+ Ogre::Quaternion visual_orientation( robot_visual_orientation.getW(), robot_visual_orientation.getX(), robot_visual_orientation.getY(), robot_visual_orientation.getZ() );
robotToOgre( visual_position );
robotToOgre( visual_orientation );
- libTF::Position robot_collision_position = link->globalTrans.getPosition();
- libTF::Quaternion robot_collision_orientation = link->globalTrans.getQuaternion();
- Ogre::Vector3 collision_position( robot_collision_position.x, robot_collision_position.y, robot_collision_position.z );
- Ogre::Quaternion collision_orientation( robot_collision_orientation.w, robot_collision_orientation.x, robot_collision_orientation.y, robot_collision_orientation.z );
+ btVector3 robot_collision_position = link->globalTrans.getOrigin();
+ btQuaternion robot_collision_orientation = link->globalTrans.getRotation();
+ Ogre::Vector3 collision_position( robot_collision_position.getX(), robot_collision_position.getY(), robot_collision_position.getZ() );
+ Ogre::Quaternion collision_orientation( robot_collision_orientation.getW(), robot_collision_orientation.getX(), robot_collision_orientation.getY(), robot_collision_orientation.getZ() );
robotToOgre( collision_position );
robotToOgre( collision_orientation );
Modified: pkg/trunk/world_models/world_3d_map/manifest.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/manifest.xml 2008-12-15 00:40:22 UTC (rev 8082)
+++ pkg/trunk/world_models/world_3d_map/manifest.xml 2008-12-15 02:19:31 UTC (rev 8083)
@@ -9,7 +9,7 @@
<depend package="laser_scan" />
<depend package="random_utils" />
<depend package="wg_robot_description_parser" />
-<depend package="planning_models_with_libTF" />
+<depend package="planning_models" />
<depend package="collision_space" />
<depend package="planning_node_util" />
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
