Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[10400] pkg/trunk
|
From: <is...@us...> - 2009-01-30 06:58:43
|
Revision: 10400
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10400&view=rev
Author: isucan
Date: 2009-01-30 06:58:36 +0000 (Fri, 30 Jan 2009)
Log Message:
-----------
added padding option for collision space
Modified Paths:
--------------
pkg/trunk/util/self_watch/self_watch.cpp
pkg/trunk/util/self_watch/self_watch.launch
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp
Modified: pkg/trunk/util/self_watch/self_watch.cpp
===================================================================
--- pkg/trunk/util/self_watch/self_watch.cpp 2009-01-30 05:47:21 UTC (rev 10399)
+++ pkg/trunk/util/self_watch/self_watch.cpp 2009-01-30 06:58:36 UTC (rev 10400)
@@ -59,6 +59,7 @@
// increase the robot parts by x%, to detect collisions before they happen
param("self_collision_scale_factor", m_scaling, 1.2);
+ param("self_collision_padding", m_padding, 0.05);
// load the string description of the robot
std::string content;
@@ -111,7 +112,7 @@
}
// add the robot model to the collision space
- m_collisionSpace->addRobotModel(m_kmodel, links, m_scaling);
+ m_collisionSpace->addRobotModel(m_kmodel, links, m_scaling, m_padding);
// get the self collision groups and add them to the collision space
@@ -131,7 +132,7 @@
if (nscgroups == 0)
ROS_WARN("No self-collision checking enabled");
- ROS_INFO("Self-collision monitor is active, with scaling %g", m_scaling);
+ ROS_INFO("Self-collision monitor is active, with scaling %g, padding %g", m_scaling, m_padding);
}
else
ROS_ERROR("Unable to parse robot description");
@@ -203,6 +204,7 @@
// small factor; when a collision is found between the inflated
// parts, the robot should take action to preserve itself
double m_scaling;
+ double m_padding;
// the complete robot state
planning_models::KinematicModel::StateParams *m_robotState;
Modified: pkg/trunk/util/self_watch/self_watch.launch
===================================================================
--- pkg/trunk/util/self_watch/self_watch.launch 2009-01-30 05:47:21 UTC (rev 10399)
+++ pkg/trunk/util/self_watch/self_watch.launch 2009-01-30 06:58:36 UTC (rev 10400)
@@ -1,4 +1,5 @@
<launch>
- <param name="self_collision_scale_factor" type="double" value="1.15" />
+ <param name="self_collision_scale_factor" type="double" value="1.05" />
+ <param name="self_collision_padding" type="double" value="0.05" />
<node pkg="self_watch" type="self_watch" args="robot_description:=robotdesc/pr2" output="screen" />
</launch>
\ No newline at end of file
Modified: pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-30 05:47:21 UTC (rev 10399)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environment.h 2009-01-30 06:58:36 UTC (rev 10400)
@@ -109,8 +109,10 @@
/** Add a robot model. Ignore robot links if their name is not
specified in the string vector. The scale argument can be
used to increase or decrease the size of the robot's
- bodies */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale = 1.0);
+ bodies (multiplicative factor). The padding can be used to
+ increase or decrease the robot's bodies with by an
+ additive term */
+ virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id) = 0;
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-01-30 05:47:21 UTC (rev 10399)
+++ pkg/trunk/world_models/robot_models/collision_space/include/collision_space/environmentODE.h 2009-01-30 06:58:36 UTC (rev 10400)
@@ -97,8 +97,13 @@
/** 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, double scale = 1.0);
+ /** Add a robot model. Ignore robot links if their name is not
+ specified in the string vector. The scale argument can be
+ used to increase or decrease the size of the robot's
+ bodies (multiplicative factor). The padding can be used to
+ increase or decrease the robot's bodies with by an
+ additive term */
+ virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale = 1.0, double padding = 0.0);
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id);
@@ -227,11 +232,12 @@
{
std::vector< kGeom* > linkGeom;
double scale;
+ double padding;
dSpaceID space;
std::vector< std::vector<unsigned int> > selfCollision;
};
- dGeomID createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape, double scale) const;
+ dGeomID createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape, double scale, double padding) const;
void updateGeom(dGeomID geom, btTransform &pose) const;
void freeMemory(void);
Modified: pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-30 05:47:21 UTC (rev 10399)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environment.cpp 2009-01-30 06:58:36 UTC (rev 10400)
@@ -41,7 +41,7 @@
m_verbose = verbose;
}
-unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale)
+unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale, double padding)
{
unsigned int pos = m_models.size();
m_models.push_back(model);
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-01-30 05:47:21 UTC (rev 10399)
+++ pkg/trunk/world_models/robot_models/collision_space/src/collision_space/environmentODE.cpp 2009-01-30 06:58:36 UTC (rev 10400)
@@ -54,15 +54,16 @@
dSpaceDestroy(m_spaceBasicGeoms);
}
-unsigned int collision_space::EnvironmentModelODE::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale)
+unsigned int collision_space::EnvironmentModelODE::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links, double scale, double padding)
{
- unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links, scale);
+ unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links, scale, padding);
if (m_modelsGeom.size() <= id)
{
m_modelsGeom.resize(id + 1);
m_modelsGeom[id].space = dHashSpaceCreate(0);
m_modelsGeom[id].scale = scale;
+ m_modelsGeom[id].padding = padding;
}
std::map<std::string, bool> exists;
@@ -85,12 +86,12 @@
kGeom *kg = new kGeom();
kg->link = robot->links[i];
kg->enabled = true;
- dGeomID g = createODEGeom(m_modelsGeom[id].space, robot->links[i]->shape, scale);
+ dGeomID g = createODEGeom(m_modelsGeom[id].space, robot->links[i]->shape, scale, padding);
assert(g);
kg->geom.push_back(g);
for (unsigned int k = 0 ; k < kg->link->attachedBodies.size() ; ++k)
{
- dGeomID ga = createODEGeom(m_modelsGeom[id].space, kg->link->attachedBodies[k]->shape, scale);
+ dGeomID ga = createODEGeom(m_modelsGeom[id].space, kg->link->attachedBodies[k]->shape, scale, padding);
assert(ga);
kg->geom.push_back(ga);
}
@@ -100,26 +101,26 @@
return id;
}
-dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape, double scale) const
+dGeomID collision_space::EnvironmentModelODE::createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape, double scale, double padding) const
{
dGeomID g = NULL;
switch (shape->type)
{
case planning_models::KinematicModel::Shape::SPHERE:
{
- g = dCreateSphere(space, static_cast<planning_models::KinematicModel::Sphere*>(shape)->radius * scale);
+ g = dCreateSphere(space, static_cast<planning_models::KinematicModel::Sphere*>(shape)->radius * scale + padding);
}
break;
case planning_models::KinematicModel::Shape::BOX:
{
const double *size = static_cast<planning_models::KinematicModel::Box*>(shape)->size;
- g = dCreateBox(space, size[0] * scale, size[1] * scale, size[2] * scale);
+ g = dCreateBox(space, size[0] * scale + padding, size[1] * scale + padding, size[2] * scale + padding);
}
break;
case planning_models::KinematicModel::Shape::CYLINDER:
{
- g = dCreateCylinder(space, static_cast<planning_models::KinematicModel::Cylinder*>(shape)->radius * scale,
- static_cast<planning_models::KinematicModel::Cylinder*>(shape)->length * scale);
+ g = dCreateCylinder(space, static_cast<planning_models::KinematicModel::Cylinder*>(shape)->radius * scale + padding,
+ static_cast<planning_models::KinematicModel::Cylinder*>(shape)->length * scale + padding);
}
break;
default:
@@ -154,7 +155,7 @@
const unsigned int nab = kg->link->attachedBodies.size();
for (unsigned int k = 0 ; k < nab ; ++k)
{
- dGeomID ga = createODEGeom(m_modelsGeom[model_id].space, kg->link->attachedBodies[k]->shape, m_modelsGeom[model_id].scale);
+ dGeomID ga = createODEGeom(m_modelsGeom[model_id].space, kg->link->attachedBodies[k]->shape, m_modelsGeom[model_id].scale, m_modelsGeom[model_id].padding);
assert(ga);
kg->geom.push_back(ga);
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
