Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[11985] pkg/trunk
|
From: <is...@us...> - 2009-03-02 17:51:11
|
Revision: 11985
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11985&view=rev
Author: isucan
Date: 2009-03-02 17:51:01 +0000 (Mon, 02 Mar 2009)
Log Message:
-----------
cleaning up the way we deal with shapes
Modified Paths:
--------------
pkg/trunk/deprecated/robot_filter/include/robot_filter/RobotFilter.h
pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp
pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
pkg/trunk/motion_planning/kinematic_planning/src/display_planner_collision_model.cpp
pkg/trunk/motion_planning/test_collision_space/CMakeLists.txt
pkg/trunk/motion_planning/test_collision_space/manifest.xml
pkg/trunk/world_models/robot_models/collision_space/CMakeLists.txt
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
pkg/trunk/world_models/robot_models/planning_models/src/kinematic.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/test_collision_space/src/test_cs_point_inclusion.cpp
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/point_inclusion.h
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/point_inclusion.cpp
pkg/trunk/world_models/robot_models/collision_space/test/test_point_inclusion.cpp
pkg/trunk/world_models/robot_models/planning_models/include/planning_models/shapes.h
Removed Paths:
-------------
pkg/trunk/motion_planning/test_collision_space/src/test_cs_util.cpp
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/util.h
pkg/trunk/world_models/robot_models/collision_space/test/test_util.cpp
Modified: pkg/trunk/deprecated/robot_filter/include/robot_filter/RobotFilter.h
===================================================================
--- pkg/trunk/deprecated/robot_filter/include/robot_filter/RobotFilter.h 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/deprecated/robot_filter/include/robot_filter/RobotFilter.h 2009-03-02 17:51:01 UTC (rev 11985)
@@ -35,7 +35,7 @@
/** \author Ioan Sucan */
-#include <collision_space/util.h>
+#include <collision_space/point_inclusion.h>
#include <robot_model/knode.h>
@@ -73,7 +73,7 @@
class RobotFilter {
private:
struct RobotPart {
- collision_space::bodies::Shape *body;
+ collision_space::bodies::Body *body;
planning_models::KinematicModel::Link *link;
};
std::vector<RobotPart> m_selfSeeParts;
Modified: pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp
===================================================================
--- pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/deprecated/robot_filter/src/robot_filter.cpp 2009-03-02 17:51:01 UTC (rev 11985)
@@ -77,37 +77,8 @@
if (link)
{
RobotPart rp = { NULL, link };
-
- switch (link->shape->type)
- {
- case planning_models::KinematicModel::Shape::BOX:
- rp.body = new collision_space::bodies::Box();
- {
- const double* size = static_cast<planning_models::KinematicModel::Box*>(link->shape)->size;
- rp.body->setDimensions(size);
- }
- break;
- case planning_models::KinematicModel::Shape::SPHERE:
- rp.body = new collision_space::bodies::Sphere();
- {
- double size[1];
- size[0] = static_cast<planning_models::KinematicModel::Sphere*>(link->shape)->radius;
- rp.body->setDimensions(size);
- }
- break;
- case planning_models::KinematicModel::Shape::CYLINDER:
- rp.body = new collision_space::bodies::Cylinder();
- {
- double size[2];
- size[0] = static_cast<planning_models::KinematicModel::Cylinder*>(link->shape)->length;
- size[1] = static_cast<planning_models::KinematicModel::Cylinder*>(link->shape)->radius;
- rp.body->setDimensions(size);
- }
- break;
- default:
- break;
- }
-
+ rp.body = collision_space::bodies::createBodyFromShape(link->shape);
+
if (!rp.body)
{
fprintf(stderr, "Unknown body type: %d\n", link->shape->type);
Modified: pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/motion_planning/kinematic_planning/include/kinematic_planning/CollisionSpaceMonitor.h 2009-03-02 17:51:01 UTC (rev 11985)
@@ -151,7 +151,7 @@
link->attachedBodies[i]->attachTrans.setOrigin(btVector3(centerP.point.x, centerP.point.y, centerP.point.z));
// this is a HACK! we should have orientation
- planning_models::KinematicModel::Box *box = new planning_models::KinematicModel::Box();
+ planning_models::shapes::Box *box = new planning_models::shapes::Box();
box->size[0] = m_attachedObject.objects[i].max_bound.x - m_attachedObject.objects[i].min_bound.x;
box->size[1] = m_attachedObject.objects[i].max_bound.y - m_attachedObject.objects[i].min_bound.y;
box->size[2] = m_attachedObject.objects[i].max_bound.z - m_attachedObject.objects[i].min_bound.z;
Modified: pkg/trunk/motion_planning/kinematic_planning/src/display_planner_collision_model.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/display_planner_collision_model.cpp 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/motion_planning/kinematic_planning/src/display_planner_collision_model.cpp 2009-03-02 17:51:01 UTC (rev 11985)
@@ -134,7 +134,7 @@
header.frame_id = link->name;
for (unsigned int i = 0 ; i < link->attachedBodies.size() ; ++i)
{
- planning_models::KinematicModel::Box *box = dynamic_cast<planning_models::KinematicModel::Box*>(link->attachedBodies[i]->shape);
+ planning_models::shapes::Box *box = dynamic_cast<planning_models::shapes::Box*>(link->attachedBodies[i]->shape);
if (box)
{
btVector3 &v = link->attachedBodies[i]->attachTrans.getOrigin();
Modified: pkg/trunk/motion_planning/test_collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/test_collision_space/CMakeLists.txt 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/motion_planning/test_collision_space/CMakeLists.txt 2009-03-02 17:51:01 UTC (rev 11985)
@@ -2,4 +2,4 @@
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
rospack(test_collision_space)
-rospack_add_executable(test_cs_util src/test_cs_util.cpp)
+rospack_add_executable(test_cs_point_inclusion src/test_cs_point_inclusion.cpp)
Modified: pkg/trunk/motion_planning/test_collision_space/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/test_collision_space/manifest.xml 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/motion_planning/test_collision_space/manifest.xml 2009-03-02 17:51:01 UTC (rev 11985)
@@ -12,6 +12,7 @@
<depend package="roslib"/>
<depend package="robot_msgs"/>
+ <depend package="planning_models"/>
<depend package="collision_space"/>
</package>
Copied: pkg/trunk/motion_planning/test_collision_space/src/test_cs_point_inclusion.cpp (from rev 11980, pkg/trunk/motion_planning/test_collision_space/src/test_cs_util.cpp)
===================================================================
--- pkg/trunk/motion_planning/test_collision_space/src/test_cs_point_inclusion.cpp (rev 0)
+++ pkg/trunk/motion_planning/test_collision_space/src/test_cs_point_inclusion.cpp 2009-03-02 17:51:01 UTC (rev 11985)
@@ -0,0 +1,258 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include <cstdlib>
+#include <ros/node.h>
+#include <ros/publisher.h>
+#include <ros/time.h>
+#include <collision_space/environmentODE.h>
+#include <algorithm>
+#include <robot_msgs/VisualizationMarker.h>
+#include <roslib/Time.h>
+#include <collision_space/point_inclusion.h>
+using namespace collision_space;
+
+const int TEST_TIMES = 3;
+const int TEST_POINTS = 50000;
+
+class TestVM
+{
+public:
+
+ TestVM(void) : m_node("TVM")
+ {
+ m_node.advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 10240);
+ m_id = 1;
+ }
+
+ virtual ~TestVM(void)
+ {
+ }
+
+ void sendPoint(double x, double y, double z)
+ {
+ robot_msgs::VisualizationMarker mk;
+
+ mk.header.stamp = m_tm;
+
+ mk.header.frame_id = "base_link";
+
+ mk.id = m_id++;
+ mk.type = robot_msgs::VisualizationMarker::SPHERE;
+ mk.action = robot_msgs::VisualizationMarker::ADD;
+ mk.x = x;
+ mk.y = y;
+ mk.z = z;
+
+ mk.roll = 0;
+ mk.pitch = 0;
+ mk.yaw = 0;
+
+ mk.xScale = 0.2;
+ mk.yScale = 0.2;
+ mk.zScale = 0.2;
+
+ mk.alpha = 255;
+ mk.r = 255;
+ mk.g = 10;
+ mk.b = 10;
+
+ m_node.publish("visualizationMarker", mk);
+ }
+
+ void testShape(collision_space::bodies::Body *s)
+ {
+ for (int i = 0 ; i < TEST_POINTS ; ++i)
+ {
+ double x = uniform(5.0);
+ double y = uniform(5.0);
+ double z = uniform(5.0);
+ if (!s->containsPoint(x, y, z))
+ continue;
+ sendPoint(x, y, z);
+ }
+ }
+
+ void setShapeTransformAndMarker(collision_space::bodies::Body *s,
+ robot_msgs::VisualizationMarker &mk)
+ {
+ btTransform t;
+
+ double yaw = uniform(M_PI);
+ double pitch = uniform(M_PI);
+ double roll = uniform(M_PI);
+
+ double x = uniform(3.0);
+ double y = uniform(3.0);
+ double z = uniform(3.0);
+
+ t.setRotation(btQuaternion(yaw, pitch, roll));
+ t.setOrigin(btVector3(btScalar(x), btScalar(y), btScalar(z)));
+
+ s->setPose(t);
+ s->setScale(0.99);
+
+ mk.header.stamp = m_tm;
+ mk.header.frame_id = "base_link";
+ mk.id = m_id++;
+
+ mk.action = robot_msgs::VisualizationMarker::ADD;
+
+ mk.x = x;
+ mk.y = y;
+ mk.z = z;
+
+ mk.roll = roll;
+ mk.pitch = pitch;
+ mk.yaw = yaw;
+
+ mk.alpha = 150;
+ mk.r = 0;
+ mk.g = 100;
+ mk.b = 255;
+ }
+
+ void testSphere(void)
+ {
+ planning_models::shapes::Sphere shape(2.0);
+ collision_space::bodies::Body *s = new collision_space::bodies::Sphere(&shape);
+
+ for (int i = 0 ; i < TEST_TIMES ; ++i)
+ {
+ robot_msgs::VisualizationMarker mk;
+ setShapeTransformAndMarker(s, mk);
+
+ mk.type = robot_msgs::VisualizationMarker::SPHERE;
+ mk.xScale = shape.radius*2.0;
+ mk.yScale = shape.radius*2.0;
+ mk.zScale = shape.radius*2.0;
+
+ m_node.publish("visualizationMarker", mk);
+
+ testShape(s);
+ }
+
+ delete s;
+ }
+
+ void testBox(void)
+ {
+ planning_models::shapes::Box shape(2.0, 1.33, 1.5);
+ collision_space::bodies::Body *s = new collision_space::bodies::Box(&shape);
+
+ for (int i = 0 ; i < TEST_TIMES ; ++i)
+ {
+ robot_msgs::VisualizationMarker mk;
+ setShapeTransformAndMarker(s, mk);
+
+ mk.type = robot_msgs::VisualizationMarker::CUBE;
+ mk.xScale = shape.size[0]; // length
+ mk.yScale = shape.size[1]; // width
+ mk.zScale = shape.size[2]; // height
+
+ m_node.publish("visualizationMarker", mk);
+
+ testShape(s);
+ }
+
+ delete s;
+ }
+
+ void testCylinder(void)
+ {
+ planning_models::shapes::Cylinder shape(0.5, 2.5);
+ collision_space::bodies::Body *s = new collision_space::bodies::Cylinder(&shape);
+
+ for (int i = 0 ; i < TEST_TIMES ; ++i)
+ {
+ robot_msgs::VisualizationMarker mk;
+ setShapeTransformAndMarker(s, mk);
+
+ mk.type = robot_msgs::VisualizationMarker::CUBE;
+ mk.xScale = shape.radius * 2.0; // radius
+ mk.yScale = shape.radius * 2.0; // radius
+ mk.zScale = shape.length; //length
+
+ m_node.publish("visualizationMarker", mk);
+
+ testShape(s);
+ }
+
+ delete s;
+ }
+
+ void run()
+ {
+ ros::Duration d;
+ d.fromSec(2);
+ d.sleep();
+
+ m_tm = ros::Time::now();
+
+ testSphere();
+ testBox();
+ testCylinder();
+
+ m_node.spin();
+ m_node.shutdown();
+
+ }
+
+protected:
+
+ // return a random number (uniform distribution)
+ // between -magnitude and magnitude
+ double uniform(double magnitude)
+ {
+ return (2.0 * drand48() - 1.0) * magnitude;
+ }
+
+ ros::Node m_node;
+ ros::Time m_tm;
+ int m_id;
+
+};
+
+
+int main(int argc, char **argv)
+{
+ ros::init(argc, argv);
+
+ TestVM tvm;
+ tvm.run();
+
+ return 0;
+}
Deleted: pkg/trunk/motion_planning/test_collision_space/src/test_cs_util.cpp
===================================================================
--- pkg/trunk/motion_planning/test_collision_space/src/test_cs_util.cpp 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/motion_planning/test_collision_space/src/test_cs_util.cpp 2009-03-02 17:51:01 UTC (rev 11985)
@@ -1,261 +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 */
-
-#include <cstdlib>
-#include <ros/node.h>
-#include <ros/publisher.h>
-#include <ros/time.h>
-#include <collision_space/environmentODE.h>
-#include <algorithm>
-#include <robot_msgs/VisualizationMarker.h>
-#include <roslib/Time.h>
-#include <collision_space/util.h>
-using namespace collision_space;
-
-const int TEST_TIMES = 3;
-const int TEST_POINTS = 50000;
-
-class TestVM
-{
-public:
-
- TestVM(void) : m_node("TVM")
- {
- m_node.advertise<robot_msgs::VisualizationMarker>("visualizationMarker", 10240);
- m_id = 1;
- }
-
- virtual ~TestVM(void)
- {
- }
-
- void sendPoint(double x, double y, double z)
- {
- robot_msgs::VisualizationMarker mk;
-
- mk.header.stamp = m_tm;
-
- mk.header.frame_id = "base_link";
-
- mk.id = m_id++;
- mk.type = robot_msgs::VisualizationMarker::SPHERE;
- mk.action = robot_msgs::VisualizationMarker::ADD;
- mk.x = x;
- mk.y = y;
- mk.z = z;
-
- mk.roll = 0;
- mk.pitch = 0;
- mk.yaw = 0;
-
- mk.xScale = 0.2;
- mk.yScale = 0.2;
- mk.zScale = 0.2;
-
- mk.alpha = 255;
- mk.r = 255;
- mk.g = 10;
- mk.b = 10;
-
- m_node.publish("visualizationMarker", mk);
- }
-
- void testShape(collision_space::bodies::Shape *s)
- {
- for (int i = 0 ; i < TEST_POINTS ; ++i)
- {
- double x = uniform(5.0);
- double y = uniform(5.0);
- double z = uniform(5.0);
- if (!s->containsPoint(x, y, z))
- continue;
- sendPoint(x, y, z);
- }
- }
-
- void setShapeTransformAndMarker(collision_space::bodies::Shape *s,
- robot_msgs::VisualizationMarker &mk)
- {
- btTransform t;
-
- double yaw = uniform(M_PI);
- double pitch = uniform(M_PI);
- double roll = uniform(M_PI);
-
- double x = uniform(3.0);
- double y = uniform(3.0);
- double z = uniform(3.0);
-
- t.setRotation(btQuaternion(yaw, pitch, roll));
- t.setOrigin(btVector3(btScalar(x), btScalar(y), btScalar(z)));
-
- s->setPose(t);
- s->setScale(0.99);
-
- mk.header.stamp = m_tm;
- mk.header.frame_id = "base_link";
- mk.id = m_id++;
-
- mk.action = robot_msgs::VisualizationMarker::ADD;
-
- mk.x = x;
- mk.y = y;
- mk.z = z;
-
- mk.roll = roll;
- mk.pitch = pitch;
- mk.yaw = yaw;
-
- mk.alpha = 150;
- mk.r = 0;
- mk.g = 100;
- mk.b = 255;
- }
-
- void testSphere(void)
- {
- collision_space::bodies::Shape *s = new collision_space::bodies::Sphere();
- double radius[1] = {2.0};
- s->setDimensions(radius);
-
- for (int i = 0 ; i < TEST_TIMES ; ++i)
- {
- robot_msgs::VisualizationMarker mk;
- setShapeTransformAndMarker(s, mk);
-
- mk.type = robot_msgs::VisualizationMarker::SPHERE;
- mk.xScale = radius[0]*2.0;
- mk.yScale = radius[0]*2.0;
- mk.zScale = radius[0]*2.0;
-
- m_node.publish("visualizationMarker", mk);
-
- testShape(s);
- }
-
- delete s;
- }
-
- void testBox(void)
- {
- collision_space::bodies::Shape *s = new collision_space::bodies::Box();
- double dims[3] = {2.0, 1.33, 1.5};
- s->setDimensions(dims);
-
- for (int i = 0 ; i < TEST_TIMES ; ++i)
- {
- robot_msgs::VisualizationMarker mk;
- setShapeTransformAndMarker(s, mk);
-
- mk.type = robot_msgs::VisualizationMarker::CUBE;
- mk.xScale = dims[0]; // length
- mk.yScale = dims[1]; // width
- mk.zScale = dims[2]; // height
-
- m_node.publish("visualizationMarker", mk);
-
- testShape(s);
- }
-
- delete s;
- }
-
- void testCylinder(void)
- {
- collision_space::bodies::Shape *s = new collision_space::bodies::Cylinder();
- double dims[2] = {2.5, 0.5};
- s->setDimensions(dims);
-
- for (int i = 0 ; i < TEST_TIMES ; ++i)
- {
- robot_msgs::VisualizationMarker mk;
- setShapeTransformAndMarker(s, mk);
-
- mk.type = robot_msgs::VisualizationMarker::CUBE;
- mk.xScale = dims[1] * 2.0; // radius
- mk.yScale = dims[1] * 2.0; // radius
- mk.zScale = dims[0]; //length
-
- m_node.publish("visualizationMarker", mk);
-
- testShape(s);
- }
-
- delete s;
- }
-
- void run()
- {
- ros::Duration d;
- d.fromSec(2);
- d.sleep();
-
- m_tm = ros::Time::now();
-
- testSphere();
- testBox();
- testCylinder();
-
- m_node.spin();
- m_node.shutdown();
-
- }
-
-protected:
-
- // return a random number (uniform distribution)
- // between -magnitude and magnitude
- double uniform(double magnitude)
- {
- return (2.0 * drand48() - 1.0) * magnitude;
- }
-
- ros::Node m_node;
- ros::Time m_tm;
- int m_id;
-
-};
-
-
-int main(int argc, char **argv)
-{
- ros::init(argc, argv);
-
- TestVM tvm;
- tvm.run();
-
- return 0;
-}
Modified: pkg/trunk/world_models/robot_models/collision_space/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/CMakeLists.txt 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/world_models/robot_models/collision_space/CMakeLists.txt 2009-03-02 17:51:01 UTC (rev 11985)
@@ -6,12 +6,13 @@
set(CMAKE_BUILD_TYPE Release)
-rospack_add_library(collision_space src/collision_space/environment.cpp
- src/collision_space/output.cpp
+rospack_add_library(collision_space src/collision_space/output.cpp
+ src/collision_space/point_inclusion.cpp
+ src/collision_space/environment.cpp
src/collision_space/environmentODE.cpp)
rospack_link_boost(collision_space thread)
# Unit tests
-rospack_add_gtest(test_util test/test_util.cpp)
-target_link_libraries(test_util collision_space)
+rospack_add_gtest(test_point_inclusion test/test_point_inclusion.cpp)
+target_link_libraries(test_point_inclusion collision_space)
Modified: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h 2009-03-02 17:51:01 UTC (rev 11985)
@@ -237,7 +237,7 @@
std::vector< std::vector<unsigned int> > selfCollision;
};
- dGeomID createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape, double scale, double padding) const;
+ dGeomID createODEGeom(dSpaceID space, planning_models::shapes::Shape *shape, double scale, double padding) const;
void updateGeom(dGeomID geom, btTransform &pose) const;
void freeMemory(void);
Added: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/point_inclusion.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/point_inclusion.h (rev 0)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/point_inclusion.h 2009-03-02 17:51:01 UTC (rev 11985)
@@ -0,0 +1,321 @@
+/*********************************************************************
+* 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 COLLISION_SPACE_POINT_INCLUSION_
+#define COLLISION_SPACE_POINT_INCLUSION_
+
+#include <planning_models/shapes.h>
+#include <LinearMath/btTransform.h>
+#include <cmath>
+
+/**
+ This set of classes allows quickly detecting whether a given point
+ is inside an object or not. Only basic (simple) types of objects
+ are supported: spheres, cylinders, boxes. This capability is useful
+ when removing points from inside the robot (when the robot sees its
+ arms, for example).
+*/
+
+namespace collision_space
+{
+
+ namespace bodies
+ {
+
+ class Body
+ {
+ public:
+ Body(void)
+ {
+ m_scale = 1.0;
+ m_padding = 0.0;
+ m_pose.setIdentity();
+ }
+
+ virtual ~Body(void)
+ {
+ }
+
+ void setScale(double scale)
+ {
+ m_scale = scale;
+ updateInternalData();
+ }
+
+ double getScale(void) const
+ {
+ return m_scale;
+ }
+
+ void setPadding(double padd)
+ {
+ m_padding = padd;
+ updateInternalData();
+ }
+
+ double getPadding(void) const
+ {
+ return m_padding;
+ }
+
+ void setPose(const btTransform &pose)
+ {
+ m_pose = pose;
+ updateInternalData();
+ }
+
+ const btTransform& getPose(void) const
+ {
+ return m_pose;
+ }
+
+ void setDimensions(const planning_models::shapes::Shape *shape)
+ {
+ useDimensions(shape);
+ updateInternalData();
+ }
+
+ bool containsPoint(double x, double y, double z) const
+ {
+ return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const = 0;
+
+ protected:
+
+ virtual void updateInternalData(void) = 0;
+ virtual void useDimensions(const planning_models::shapes::Shape *shape) = 0;
+
+ btTransform m_pose;
+ double m_scale;
+ double m_padding;
+ };
+
+ class Sphere : public Body
+ {
+ public:
+ Sphere(void) : Body()
+ {
+ m_radius = 0.0;
+ }
+
+ Sphere(const planning_models::shapes::Shape *shape) : Body()
+ {
+ setDimensions(shape);
+ }
+
+ virtual ~Sphere(void)
+ {
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const
+ {
+ return (m_center - p).length2() < m_radius2;
+ }
+
+ protected:
+
+ virtual void useDimensions(const planning_models::shapes::Shape *shape) // radius
+ {
+ m_radius = static_cast<const planning_models::shapes::Sphere*>(shape)->radius;
+ }
+
+ virtual void updateInternalData(void)
+ {
+ m_radius2 = m_radius * m_scale + m_padding;
+ m_radius2 = m_radius2 * m_radius2;
+ m_center = m_pose.getOrigin();
+ }
+
+ btVector3 m_center;
+ double m_radius;
+ double m_radius2;
+ };
+
+ class Cylinder : public Body
+ {
+ public:
+ Cylinder(void) : Body()
+ {
+ m_length = m_radius = 0.0;
+ }
+
+ Cylinder(const planning_models::shapes::Shape *shape) : Body()
+ {
+ setDimensions(shape);
+ }
+
+ virtual ~Cylinder(void)
+ {
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const
+ {
+ btVector3 v = p - m_center;
+ double pH = v.dot(m_normalH);
+
+ if (fabs(pH) > m_length2)
+ return false;
+
+ double pB1 = v.dot(m_normalB1);
+ double remaining = m_radius2 - pB1 * pB1;
+
+ if (remaining < 0.0)
+ return false;
+ else
+ {
+ double pB2 = v.dot(m_normalB2);
+ return pB2 * pB2 < remaining;
+ }
+ }
+
+ protected:
+
+ virtual void useDimensions(const planning_models::shapes::Shape *shape) // (length, radius)
+ {
+ m_length = static_cast<const planning_models::shapes::Cylinder*>(shape)->length;
+ m_radius = static_cast<const planning_models::shapes::Cylinder*>(shape)->radius;
+ }
+
+ virtual void updateInternalData(void)
+ {
+ m_radius2 = m_radius * m_scale + m_padding;
+ m_radius2 = m_radius2 * m_radius2;
+ m_length2 = (m_scale * m_length + m_padding) / 2.0;
+ m_center = m_pose.getOrigin();
+
+ const btMatrix3x3& basis = m_pose.getBasis();
+ m_normalB1 = basis.getColumn(0);
+ m_normalB2 = basis.getColumn(1);
+ m_normalH = basis.getColumn(2);
+ }
+
+ btVector3 m_center;
+ btVector3 m_normalH;
+ btVector3 m_normalB1;
+ btVector3 m_normalB2;
+
+ double m_length;
+ double m_length2;
+ double m_radius;
+ double m_radius2;
+ };
+
+
+ class Box : public Body
+ {
+ public:
+ Box(void) : Body()
+ {
+ m_length = m_width = m_height = 0.0;
+ }
+
+ Box(const planning_models::shapes::Shape *shape) : Body()
+ {
+ setDimensions(shape);
+ }
+
+ virtual ~Box(void)
+ {
+ }
+
+ virtual bool containsPoint(const btVector3 &p) const
+ {
+ btVector3 v = p - m_center;
+ double pL = v.dot(m_normalL);
+
+ if (fabs(pL) > m_length2)
+ return false;
+
+ double pW = v.dot(m_normalW);
+
+ if (fabs(pW) > m_width2)
+ return false;
+
+ double pH = v.dot(m_normalH);
+
+ if (fabs(pH) > m_height2)
+ return false;
+
+ return true;
+ }
+
+ protected:
+
+ virtual void useDimensions(const planning_models::shapes::Shape *shape) // (x, y, z) = (length, width, height)
+ {
+ const double *size = static_cast<const planning_models::shapes::Box*>(shape)->size;
+ m_length = size[0];
+ m_width = size[1];
+ m_height = size[2];
+ }
+
+ virtual void updateInternalData(void)
+ {
+ double s2 = m_scale / 2.0;
+ double p2 = m_padding / 2.0;
+ m_length2 = m_length * s2 + p2;
+ m_width2 = m_width * s2 + p2;
+ m_height2 = m_height * s2 + p2;
+
+ m_center = m_pose.getOrigin();
+
+ const btMatrix3x3& basis = m_pose.getBasis();
+ m_normalL = basis.getColumn(0);
+ m_normalW = basis.getColumn(1);
+ m_normalH = basis.getColumn(2);
+ }
+
+ 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;
+ };
+
+ Body* createBodyFromShape(const planning_models::shapes::Shape *shape);
+
+ }
+}
+
+#endif
Deleted: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/util.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/util.h 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/util.h 2009-03-02 17:51:01 UTC (rev 11985)
@@ -1,281 +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 */
-
-#ifndef COLLISION_SPACE_UTIL_
-#define COLLISION_SPACE_UTIL_
-
-#include <LinearMath/btTransform.h>
-#include <cmath>
-
-/**
- This set of classes allows quickly detecting whether a given point
- is inside an object or not. Only basic (simple) types of objects
- are supported: spheres, cylinders, boxes. This capability is useful
- when removing points from inside the robot (when the robot sees its
- arms, for example).
-*/
-
-namespace collision_space
-{
-
- namespace bodies
- {
-
- class Shape
- {
- public:
- Shape(void)
- {
- m_scale = 1.0;
- m_pose.setIdentity();
- }
-
- virtual ~Shape(void)
- {
- }
-
- void setScale(double scale)
- {
- m_scale = scale;
- updateInternalData();
- }
-
- void setPose(const btTransform &pose)
- {
- m_pose = pose;
- updateInternalData();
- }
-
- const btTransform& getPose(void) const
- {
- return m_pose;
- }
-
- virtual void setDimensions(const double *dims)
- {
- useDimensions(dims);
- updateInternalData();
- }
-
- bool containsPoint(double x, double y, double z) const
- {
- return containsPoint(btVector3(btScalar(x), btScalar(y), btScalar(z)));
- }
-
- virtual bool containsPoint(const btVector3 &p) const = 0;
-
- protected:
-
- virtual void updateInternalData(void) = 0;
- virtual void useDimensions(const double *dims) = 0;
-
- btTransform m_pose;
- double m_scale;
- };
-
- class Sphere : public Shape
- {
- public:
- Sphere(void) : Shape()
- {
- m_radius = 0.0;
- }
-
- virtual ~Sphere(void)
- {
- }
-
- virtual bool containsPoint(const btVector3 &p) const
- {
- return (m_center - p).length2() < m_radius2;
- }
-
- protected:
-
- virtual void useDimensions(const double *dims) // radius
- {
- m_radius = dims[0];
- }
-
- virtual void updateInternalData(void)
- {
- m_radius2 = m_radius * m_radius * m_scale * m_scale;
- m_center = m_pose.getOrigin();
- }
-
- btVector3 m_center;
- double m_radius;
- double m_radius2;
- };
-
- class Cylinder : public Shape
- {
- public:
- Cylinder(void) : Shape()
- {
- m_length = m_radius = 0.0;
- }
-
- virtual ~Cylinder(void)
- {
- }
-
- virtual bool containsPoint(const btVector3 &p) const
- {
- btVector3 v = p - m_center;
- double pH = v.dot(m_normalH);
-
- if (fabs(pH) > m_length2)
- return false;
-
- double pB1 = v.dot(m_normalB1);
- double remaining = m_radius2 - pB1 * pB1;
-
- if (remaining < 0.0)
- return false;
- else
- {
- double pB2 = v.dot(m_normalB2);
- return pB2 * pB2 < remaining;
- }
- }
-
- protected:
-
- virtual void useDimensions(const double *dims) // (length, radius)
- {
- m_length = dims[0];
- m_radius = dims[1];
- }
-
- virtual void updateInternalData(void)
- {
- m_radius2 = m_radius * m_radius * m_scale * m_scale;
- m_length2 = m_scale * m_length / 2.0;
- m_center = m_pose.getOrigin();
-
- const btMatrix3x3& basis = m_pose.getBasis();
- m_normalB1 = basis.getColumn(0);
- m_normalB2 = basis.getColumn(1);
- m_normalH = basis.getColumn(2);
- }
-
- btVector3 m_center;
- btVector3 m_normalH;
- btVector3 m_normalB1;
- btVector3 m_normalB2;
-
- double m_length;
- double m_length2;
- double m_radius;
- double m_radius2;
- };
-
-
- class Box : public Shape
- {
- public:
- Box(void) : Shape()
- {
- m_length = m_width = m_height = 0.0;
- }
-
- virtual ~Box(void)
- {
- }
-
- virtual bool containsPoint(const btVector3 &p) const
- {
- btVector3 v = p - m_center;
- double pL = v.dot(m_normalL);
-
- if (fabs(pL) > m_length2)
- return false;
-
- double pW = v.dot(m_normalW);
-
- if (fabs(pW) > m_width2)
- return false;
-
- double pH = v.dot(m_normalH);
-
- if (fabs(pH) > m_height2)
- return false;
-
- return true;
- }
-
- protected:
-
- virtual void useDimensions(const double *dims) // (x, y, z) = (length, width, height)
- {
- m_length = dims[0];
- m_width = dims[1];
- m_height = dims[2];
- }
-
- virtual void updateInternalData(void)
- {
- double s2 = m_scale / 2.0;
- m_length2 = m_length * s2;
- m_width2 = m_width * s2;
- m_height2 = m_height * s2;
-
- m_center = m_pose.getOrigin();
-
- const btMatrix3x3& basis = m_pose.getBasis();
- m_normalL = basis.getColumn(0);
- m_normalW = basis.getColumn(1);
- m_normalH = basis.getColumn(2);
- }
-
- 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;
- };
-
- }
-}
-
-#endif
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-03-02 17:51:01 UTC (rev 11985)
@@ -101,26 +101,26 @@
return id;
}
-dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape, double scale, double padding) const
+dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, planning_models::shapes::Shape *shape, double scale, double padding) const
{
dGeomID g = NULL;
switch (shape->type)
{
- case planning_models::KinematicModel::Shape::SPHERE:
+ case planning_models::shapes::Shape::SPHERE:
{
- g = dCreateSphere(space, static_cast<planning_models::KinematicModel::Sphere*>(shape)->radius * scale + padding);
+ g = dCreateSphere(space, static_cast<planning_models::shapes::Sphere*>(shape)->radius * scale + padding);
}
break;
- case planning_models::KinematicModel::Shape::BOX:
+ case planning_models::shapes::Shape::BOX:
{
- const double *size = static_cast<planning_models::KinematicModel::Box*>(shape)->size;
+ const double *size = static_cast<planning_models::shapes::Box*>(shape)->size;
g = dCreateBox(space, size[0] * scale + padding, size[1] * scale + padding, size[2] * scale + padding);
}
break;
- case planning_models::KinematicModel::Shape::CYLINDER:
+ case planning_models::shapes::Shape::CYLINDER:
{
- g = dCreateCylinder(space, static_cast<planning_models::KinematicModel::Cylinder*>(shape)->radius * scale + padding,
- static_cast<planning_models::KinematicModel::Cylinder*>(shape)->length * scale + padding);
+ g = dCreateCylinder(space, static_cast<planning_models::shapes::Cylinder*>(shape)->radius * scale + padding,
+ static_cast<planning_models::shapes::Cylinder*>(shape)->length * scale + padding);
}
break;
default:
Added: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/point_inclusion.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/point_inclusion.cpp (rev 0)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/point_inclusion.cpp 2009-03-02 17:51:01 UTC (rev 11985)
@@ -0,0 +1,63 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include "collision_space/point_inclusion.h"
+#include "collision_space/output.h"
+
+static collision_space::msg::Interface MSG;
+
+collision_space::bodies::Body* collision_space::bodies::createBodyFromShape(const planning_models::shapes::Shape *shape)
+{
+ Body *body = NULL;
+
+ switch (shape->type)
+ {
+ case planning_models::shapes::Shape::BOX:
+ body = new collision_space::bodies::Box(shape);
+ break;
+ case planning_models::shapes::Shape::SPHERE:
+ body = new collision_space::bodies::Sphere(shape);
+ break;
+ case planning_models::shapes::Shape::CYLINDER:
+ body = new collision_space::bodies::Cylinder(shape);
+ break;
+ default:
+ MSG.error("Creating body from shape: Unknown shape type %d", shape->type);
+ break;
+ }
+
+ return body;
+}
Added: pkg/trunk/world_models/robot_models/collision_space/test/test_point_inclusion.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/test/test_point_inclusion.cpp (rev 0)
+++ pkg/trunk/world_models/robot_models/collision_space/test/test_point_inclusion.cpp 2009-03-02 17:51:01 UTC (rev 11985)
@@ -0,0 +1,171 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+/** \Author Ioan Sucan */
+
+#include <collision_space/point_inclusion.h>
+#include <gtest/gtest.h>
+
+TEST(SpherePointContainment, SimpleInside)
+{
+ planning_models::shapes::Sphere shape(1.0);
+ collision_space::bodies::Body* sphere = new collision_space::bodies::Sphere(&shape);
+ sphere->setScale(1.05);
+ bool contains = sphere->containsPoint(0,0,1.0);
+ delete sphere;
+ EXPECT_TRUE(contains);
+}
+
+TEST(SpherePointContainment, SimpleOutside)
+{
+ planning_models::shapes::Sphere shape(1.0);
+ collision_space::bodies::Body* sphere = new collision_space::bodies::Sphere(&shape);
+ sphere->setScale(0.95);
+ bool contains = sphere->containsPoint(0,0,1.0);
+ delete sphere;
+ EXPECT_FALSE(contains);
+}
+
+TEST(SpherePointContainment, ComplexInside)
+{
+ planning_models::shapes::Sphere shape(1.0);
+ collision_space::bodies::Body* sphere = new collision_space::bodies::Sphere(&shape);
+ sphere->setScale(0.95);
+ btTransform pose;
+ pose.setIdentity();
+ pose.setOrigin(btVector3(btScalar(1),btScalar(1),btScalar(1)));
+ sphere->setPose(pose);
+ bool contains = sphere->containsPoint(0.5,1,1.0);
+ delete sphere;
+ EXPECT_TRUE(contains);
+}
+
+TEST(SpherePointContainment, ComplexOutside)
+{
+ planning_models::shapes::Sphere shape(1.0);
+ collision_space::bodies::Body* sphere = new collision_space::bodies::Sphere(&shape);
+ sphere->setScale(0.95);
+ btTransform pose;
+ pose.setIdentity();
+ pose.setOrigin(btVector3(btScalar(1),btScalar(1),btScalar(1)));
+ sphere->setPose(pose);
+ bool contains = sphere->containsPoint(0.5,0.0,0.0);
+ delete sphere;
+ EXPECT_FALSE(contains);
+}
+
+
+TEST(BoxPointContainment, SimpleInside)
+{
+ planning_models::shapes::Box shape(1.0, 2.0, 3.0);
+ collision_space::bodies::Body* box = new collision_space::bodies::Box(&shape);
+ box->setScale(0.95);
+ bool contains = box->containsPoint(0,0,1.0);
+ delete box;
+ EXPECT_TRUE(contains);
+}
+
+
+TEST(BoxPointContainment, SimpleOutside)
+{
+ planning_models::shapes::Box shape(1.0, 2.0, 3.0);
+ collision_space::bodies::Body* box = new collision_space::bodies::Box(&shape);
+ box->setScale(0.95);
+ bool contains = box->containsPoint(0,0,3.0);
+ delete box;
+ EXPECT_FALSE(contains);
+}
+
+
+TEST(BoxPointContainment, ComplexInside)
+{
+ planning_models::shapes::Box shape(1.0, 1.0, 1.0);
+ collision_space::bodies::Body* box = new collision_space::bodies::Box(&shape);
+ box->setScale(1.01);
+ btTransform pose;
+ pose.setIdentity();
+ pose.setOrigin(btVector3(btScalar(1),btScalar(1),btScalar(1)));
+ btQuaternion quat(btVector3(btScalar(1), btScalar(0), btScalar(0)), M_PI/3.0);
+ pose.setRotation(quat);
+ box->setPose(pose);
+
+ bool contains = box->containsPoint(1.5,1.0,1.5);
+ delete box;
+ EXPECT_TRUE(contains);
+}
+
+TEST(BoxPointContainment, ComplexOutside)
+{
+ planning_models::shapes::Box shape(1.0, 1.0, 1.0);
+ collision_space::bodies::Body* box = new collision_space::bodies::Box(&shape);
+ box->setScale(1.01);
+ btTransform pose;
+ pose.setIdentity();
+ pose.setOrigin(btVector3(btScalar(1),btScalar(1),btScalar(1)));
+ btQuaternion quat(btVector3(btScalar(1), btScalar(0), btScalar(0)), M_PI/3.0);
+ pose.setRotation(quat);
+ box->setPose(pose);
+
+ bool contains = box->containsPoint(1.5,1.5,1.5);
+ delete box;
+ EXPECT_FALSE(contains);
+}
+
+TEST(CylinderPointContainment, SimpleInside)
+{
+ planning_models::shapes::Cylinder shape(1.0, 4.0);
+ collision_space::bodies::Body* cylinder = new collision_space::bodies::Cylinder(&shape);
+ cylinder->setScale(1.05);
+ bool contains = cylinder->containsPoint(0, 0, 2.0);
+ delete cylinder;
+ EXPECT_TRUE(contains);
+}
+
+
+TEST(CylinderPointContainment, SimpleOutside)
+{
+ planning_models::shapes::Cylinder shape(1.0, 4.0);
+ collision_space::bodies::Body* cylinder = new collision_space::bodies::Cylinder(&shape);
+ cylinder->setScale(0.95);
+ bool contains = cylinder->containsPoint(0,0,2.0);
+ delete cylinder;
+ EXPECT_FALSE(contains);
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
Deleted: pkg/trunk/world_models/robot_models/collision_space/test/test_util.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/test/test_util.cpp 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/world_models/robot_models/collision_space/test/test_util.cpp 2009-03-02 17:51:01 UTC (rev 11985)
@@ -1,181 +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 */
-
-#include <collision_space/util.h>
-#include <gtest/gtest.h>
-
-TEST(SpherePointContainment, SimpleInside)
-{
- collision_space::bodies::Shape* sphere = new collision_space::bodies::Sphere();
- double dims = 1.0;
- sphere->setDimensions(&dims);
- sphere->setScale(1.05);
- bool contains = sphere->containsPoint(0,0,1.0);
- delete sphere;
- EXPECT_TRUE(contains);
-}
-
-TEST(SpherePointContainment, SimpleOutside)
-{
- collision_space::bodies::Shape* sphere = new collision_space::bodies::Sphere();
- double dims = 1.0;
- sphere->setDimensions(&dims);
- sphere->setScale(0.95);
- bool contains = sphere->containsPoint(0,0,1.0);
- delete sphere;
- EXPECT_FALSE(contains);
-}
-
-TEST(SpherePointContainment, ComplexInside)
-{
- collision_space::bodies::Shape* sphere = new collision_space::bodies::Sphere();
- double dims = 1.0;
- sphere->setDimensions(&dims);
- sphere->setScale(0.95);
- btTransform pose;
- pose.setIdentity();
- pose.setOrigin(btVector3(btScalar(1),btScalar(1),btScalar(1)));
- sphere->setPose(pose);
- bool contains = sphere->containsPoint(0.5,1,1.0);
- delete sphere;
- EXPECT_TRUE(contains);
-}
-
-TEST(SpherePointContainment, ComplexOutside)
-{
- collision_space::bodies::Shape* sphere = new collision_space::bodies::Sphere();
- double dims = 1.0;
- sphere->setDimensions(&dims);
- sphere->setScale(0.95);
- btTransform pose;
- pose.setIdentity();
- pose.setOrigin(btVector3(btScalar(1),btScalar(1),btScalar(1)));
- sphere->setPose(pose);
- bool contains = sphere->containsPoint(0.5,0.0,0.0);
- delete sphere;
- EXPECT_FALSE(contains);
-}
-
-
-TEST(BoxPointContainment, SimpleInside)
-{
- 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);
- bool contains = box->containsPoint(0,0,1.0);
- delete box;
- EXPECT_TRUE(contains);
-}
-
-
-TEST(BoxPointContainment, SimpleOutside)
-{
- 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);
- bool contains = box->containsPoint(0,0,3.0);
- delete box;
- EXPECT_FALSE(contains);
-}
-
-
-TEST(BoxPointContainment, ComplexInside)
-{
- 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);
- btTransform pose;
- pose.setIdentity();
- pose.setOrigin(btVector3(btScalar(1),btScalar(1),btScalar(1)));
- btQuaternion quat(btVector3(btScalar(1), btScalar(0), btScalar(0)), M_PI/3.0);
- pose.setRotation(quat);
- box->setPose(pose);
-
- bool contains = box->containsPoint(1.5,1.0,1.5);
- delete box;
- EXPECT_TRUE(contains);
-}
-
-TEST(BoxPointContainment, ComplexOutside)
-{
- 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);
- btTransform pose;
- pose.setIdentity();
- pose.setOrigin(btVector3(btScalar(1),btScalar(1),btScalar(1)));
- btQuaternion quat(btVector3(btScalar(1), btScalar(0), btScalar(0)), M_PI/3.0);
- pose.setRotation(quat);
- box->setPose(pose);
-
- bool contains = box->containsPoint(1.5,1.5,1.5);
- delete box;
- EXPECT_FALSE(contains);
-}
-
-TEST(CylinderPointContainment, SimpleInside)
-{
- collision_space::bodies::Shape* cylinder = new collision_space::bodies::Cylinder();
- double dims[3] = {4.0, 1.0};
- cylinder->setDimensions(dims);
- cylinder->setScale(1.05);
- bool contains = cylinder->containsPoint(0, 0, 2.0);
- delete cylinder;
- EXPECT_TRUE(contains);
-}
-
-
-TEST(CylinderPointContainment, SimpleOutside)
-{
- collision_space::bodies::Shape* cylinder = new collision_space::bodies::Cylinder();
- double dims[3] = {4.0, 1.0};
- cylinder->setDimensions(dims);
- cylinder->setScale(0.95);
- bool contains = cylinder->containsPoint(0,0,2.0);
- delete cylinder;
- EXPECT_FALSE(contains);
-}
-
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}
Modified: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-03-02 17:40:16 UTC (rev 11984)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/kinematic.h 2009-03-02 17:51:01 UTC (rev 11985)
@@ -34,9 +34,11 @@
/** \author Ioan Sucan */
-#ifndef KINEMATIC_ROBOT_MODEL_
-#define KINEMATIC_ROBOT_MODEL_
+#ifndef PLANNING_MODELS_KINEMATIC_ROBOT_MODEL_
+#define PLANNING_MODELS_KINEMATIC_ROBOT_MODEL_
+#include "planning_models/shapes.h"
+
#include <urdf/URDF.h>
#include <LinearMath/btTransform.h>
#include <boost/thread/mutex.hpp>
@@ -62,64 +64,6 @@
{
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;
-
- };
-
- /** Definition of a sphere */
- class Sphere : public Shape
- {
- public:
- Sphere(void) : Shape()
- {
- type = SPHERE;
- radius = 0.0;
- }
-
- double radius;
- };
-
- /** Definition of a cylinder */
- class Cylinder : public Shape
- {
- public:
- Cylinder(void) : Shape()
- {
- type = CYLINDER;
- length = radius = 0.0;
- }
-
- 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;
@@ -299,7 +243,7 @@
btTransform attachTrans;
/** The geometry of the attached body */
- Shape* shape;
+ shapes::Shape* shape;
/** The global transform for this link (computed by forward kinematics) */
btTransform globalTrans;
@@ -355,7 +299,7 @@
btTransform constGeomTrans;
/** The geometry of the link */
- Shape *shape;
+ shapes::Shape *shape;
/** Attached bodies */
std::vector<AttachedBody*> attachedBodies;
Added: pkg/trunk/world_models/robot_models/planning_models/include/planning_models/shapes.h
===================================================================
--- pkg/trunk/world_models/robot_models/planning_models/include/planning_models/shapes.h (rev 0)
+++ pkg/trunk/world_models/robot_models/planning_models/include/planning_models/shapes.h 2009-03-02 17:51:01 UTC (rev 11985)
@@ -0,0 +1,129 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Will...
[truncated message content] |
