Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3634] pkg/trunk
|
From: <is...@us...> - 2008-08-26 20:31:41
|
Revision: 3634
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3634&view=rev
Author: isucan
Date: 2008-08-26 20:31:49 +0000 (Tue, 26 Aug 2008)
Log Message:
-----------
fixed some of the self-collision groups, added ground for collision checking, added option to limit the bodies that are tested for collision
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/environmentOctree.h
pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h
pkg/trunk/world_models/world_3d_map/src/world_3d_map.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-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h 2008-08-26 20:31:49 UTC (rev 3634)
@@ -73,9 +73,12 @@
/** Add a point cloud to the collision space */
virtual void addPointCloud(unsigned int n, const double* points, double radius = 0.01) = 0;
- /** Add a robot model */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model);
+ /** Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
+ virtual void addPlane(double a, double b, double c, double d) = 0;
+ /** 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) = 0;
@@ -84,6 +87,7 @@
/** Get the number of loaded models */
unsigned int getModelCount(void) const;
+
/** Get a specific model */
planning_models::KinematicModel* getRobotModel(unsigned int model_id) const;
@@ -107,8 +111,6 @@
/** List of loaded robot models */
std::vector<planning_models::KinematicModel*> m_models;
-
-
};
}
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-08-26 20:31:49 UTC (rev 3634)
@@ -81,9 +81,12 @@
/** Add a point cloud to the collision space */
virtual void addPointCloud(unsigned int n, const double *points, double radius = 0.01);
- /** Add a robot model */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model);
+ /** Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
+ virtual void addPlane(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);
@@ -175,6 +178,7 @@
std::vector<ModelInfo> m_kgeoms;
dSpaceID m_space;
ODECollide2 m_collide2;
+ std::vector<dGeomID> m_odeGeoms;
};
}
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-08-26 20:31:49 UTC (rev 3634)
@@ -75,9 +75,12 @@
/** Add a point cloud to the collision space */
virtual void addPointCloud(unsigned int n, const double *points, double radius = 0.01);
- /** Add a robot model */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model);
+ /** Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
+ virtual void addPlane(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);
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp 2008-08-26 20:31:49 UTC (rev 3634)
@@ -36,7 +36,7 @@
#include <collision_space/environment.h>
-unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model)
+unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links)
{
unsigned int pos = m_models.size();
m_models.push_back(model);
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-08-26 20:31:49 UTC (rev 3634)
@@ -35,6 +35,7 @@
/** \Author Ioan Sucan */
#include <collision_space/environmentODE.h>
+#include <map>
void collision_space::EnvironmentModelODE::freeMemory(void)
{
@@ -48,21 +49,33 @@
dSpaceDestroy(m_space);
}
-unsigned int collision_space::EnvironmentModelODE::addRobotModel(planning_models::KinematicModel *model)
+unsigned int collision_space::EnvironmentModelODE::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links)
{
- unsigned int id = collision_space::EnvironmentModel::addRobotModel(model);
-
+ unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links);
+
if (m_kgeoms.size() <= id)
{
m_kgeoms.resize(id + 1);
m_kgeoms[id].space = dHashSpaceCreate(0);
}
+
+ std::map<std::string, bool> exists;
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ exists[links[i]] = true;
for (unsigned int j = 0 ; j < m_models[id]->getRobotCount() ; ++j)
{
planning_models::KinematicModel::Robot *robot = m_models[id]->getRobot(j);
for (unsigned int i = 0 ; i < robot->links.size() ; ++i)
{
+ /* skip this link if we have no geometry or if the link
+ name is not specified as enabled for collision
+ checking */
+ if (!robot->links[i]->shape)
+ continue;
+ if (exists.find(robot->links[i]->name) == exists.end())
+ continue;
+
kGeom *kg = new kGeom();
kg->link = robot->links[i];
dGeomID g = createODEGeom(m_kgeoms[id].space, robot->links[i]->shape);
@@ -217,6 +230,7 @@
CollisionData cdata;
cdata.collides = false;
+ /* check self collision */
if (m_selfCollision)
{
for (int i = m_kgeoms[model_id].selfCollision.size() - 1 ; i >= 0 ; --i)
@@ -244,19 +258,49 @@
if (cdata.collides)
{
std::cout << m_kgeoms[model_id].geom[vec[j]]->link->name << " intersects with " << m_kgeoms[model_id].geom[vec[k]]->link->name << std::endl;
- goto OUT;
+ goto OUT1;
}
}
}
}
- OUT:
+ /* check collision with standalone ode bodies */
+ OUT1:
+
if (!cdata.collides)
{
+ for (int i = m_kgeoms[model_id].geom.size() - 1 ; i >= 0 ; --i)
+ {
+ dGeomID g1 = m_kgeoms[model_id].geom[i]->geom;
+ dReal aabb1[6];
+ dGeomGetAABB(g1, aabb1);
+ for (int j = m_odeGeoms.size() - 1 ; j >= 0 ; --j)
+ {
+ dGeomID g2 = m_odeGeoms[j];
+ dReal aabb2[6];
+ dGeomGetAABB(g2, aabb2);
+ if (!(aabb1[2] > aabb2[3] ||
+ aabb1[3] < aabb2[2] ||
+ aabb1[4] > aabb2[5] ||
+ aabb1[5] < aabb2[4]))
+ dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ if (cdata.collides)
+ goto OUT2;
+ }
+ }
+ }
+
+ /* check collision with pointclouds */
+ OUT2:
+
+ if (!cdata.collides)
+ {
m_collide2.setup();
for (int i = m_kgeoms[model_id].geom.size() - 1 ; i >= 0 && !cdata.collides ; --i)
m_collide2.collide(m_kgeoms[model_id].geom[i]->geom, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ if (cdata.collides)
+ std::cout << "Pointcloud intersection" << std::endl;
}
return cdata.collides;
@@ -274,6 +318,12 @@
m_collide2.setup();
}
+void collision_space::EnvironmentModelODE::addPlane(double a, double b, double c, double d)
+{
+ dGeomID g = dCreatePlane(m_space, a, b, c, d);
+ m_odeGeoms.push_back(g);
+}
+
void collision_space::EnvironmentModelODE::clearObstacles(void)
{
m_collide2.clear();
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-08-26 20:31:49 UTC (rev 3634)
@@ -36,12 +36,22 @@
#include <collision_space/environmentOctree.h>
-unsigned int collision_space::EnvironmentModelOctree::addRobotModel(planning_models::KinematicModel *model)
+unsigned int collision_space::EnvironmentModelOctree::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links)
{
- unsigned int id = collision_space::EnvironmentModel::addRobotModel(model);
+ unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links);
if (id <= m_modelLinks.size())
m_modelLinks.resize(id + 1);
- m_models[id]->getLinks(m_modelLinks[id]);
+
+ 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;
}
@@ -138,6 +148,11 @@
return &m_octree;
}
+void collision_space::EnvironmentModelOctree::addPlane(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/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-26 20:31:49 UTC (rev 3634)
@@ -190,10 +190,11 @@
req.goal_constraints[0].robot_link = "wrist_flex_left";
req.goal_constraints[0].pose.position.x = 0.5;
req.goal_constraints[0].pose.position.y = 0.3;
- req.goal_constraints[0].pose.position.z = 1.0;
+ req.goal_constraints[0].pose.position.z = -1.0;
req.goal_constraints[0].position_distance = 0.01;
// an example of constraints: do not move the elbow too much
+ /*
req.constraints.set_pose_size(1);
req.constraints.pose[0].type = robot_msgs::PoseConstraint::ONLY_POSITION;
req.constraints.pose[0].robot_link = "elbow_flex_left";
@@ -201,7 +202,7 @@
req.constraints.pose[0].pose.position.y = 0.188;
req.constraints.pose[0].pose.position.z = 0.74;
req.constraints.pose[0].position_distance = 0.01;
-
+ */
req.allowed_time = 3.0;
req.params.volumeMin.x = -5.0 + m_basePos[0]; req.params.volumeMin.y = -5.0 + m_basePos[1]; req.params.volumeMin.z = 0.0;
Modified: pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp 2008-08-26 20:31:49 UTC (rev 3634)
@@ -496,14 +496,10 @@
{
unsigned int pos = m_pos[name];
for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
- {
- std::cout << joint->name << ": Setting param " << i << " to " << params[i] << std::endl;
m_params[pos + i] = params[i];
- }
-
}
else
- fprintf(stderr, "Unknown joint: '%s'\n", name.c_str());
+ std::cerr << "Unknown joint: '" << name << "'" << std::endl;
}
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-26 20:31:49 UTC (rev 3634)
@@ -94,6 +94,7 @@
else
m_collisionSpace = new collision_space::EnvironmentModelODE();
m_collisionSpace->setSelfCollision(true);
+ m_collisionSpace->addPlane(0.0, 0.0, 1.0, -0.01);
m_sphereSize = 0.03;
@@ -114,8 +115,12 @@
NodeRobotModel::setRobotDescription(file);
if (m_kmodel)
{
+ std::vector<std::string> links;
+ robot_desc::URDF::Group *g = file->getGroup("collision_check");
+ if (g && g->hasFlag("collision"))
+ links = g->linkNames;
m_collisionSpace->lock();
- unsigned int cid = m_collisionSpace->addRobotModel(m_kmodel);
+ unsigned int cid = m_collisionSpace->addRobotModel(m_kmodel, links);
m_collisionSpace->unlock();
addSelfCollisionGroups(cid, file);
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-26 20:31:49 UTC (rev 3634)
@@ -4,7 +4,7 @@
<!-- Define the parts of the robot that the robot's scanners can see -->
- <group name="self_see" flags="mapping">
+ <group name="self_see" flags="collision">
upperarm_roll_left
upperarm_roll_right
elbow_flex_left
@@ -21,97 +21,82 @@
finger_r_right
</group>
- <!-- Define groups to check for self collision -->
-
- <group name="collisions1" flags="self_collision">
- shoulder_pan_right
+ <!-- Define the parts of the robot that the collision checker should care about -->
+ <group name="collision_check" flags="collision">
+ base
torso
- </group>
-
-
- <group name="collisions2" flags="self_collision">
+ shoulder_pan_right
shoulder_pitch_right
- torso
+ shoulder_pan_left
+ shoulder_pitch_left
+ upperarm_roll_left
+ upperarm_roll_right
+ elbow_flex_left
+ elbow_flex_right
+ forearm_roll_left
+ forearm_roll_right
+ wrist_flex_left
+ wrist_flex_right
+ gripper_roll_left
+ gripper_roll_right
+ finger_l_left
+ finger_r_left
+ finger_l_right
+ finger_r_right
+ head_pan
+ head_tilt
</group>
+
+ <!-- Define groups to check for self collision -->
- <group name="collisions3" flags="self_collision">
+ <group name="collisions1" flags="self_collision">
upperarm_roll_right
torso
</group>
- <group name="collisions4" flags="self_collision">
+ <group name="collisions2" flags="self_collision">
elbow_flex_right
base
</group>
- <group name="collisions5" flags="self_collision">
+ <group name="collisions3" flags="self_collision">
forearm_roll_right
base
</group>
- <group name="collisions6" flags="self_collision">
+ <group name="collisions4" flags="self_collision">
wrist_flex_right
base
</group>
- <group name="collisions7" flags="self_collision">
+ <group name="collisions5" flags="self_collision">
gripper_roll_right
base
</group>
- <group name="collisions8" flags="self_collision">
- upperarm_roll_right
- shoulder_pan_left
- shoulder_pitch_left
+ <group name="collisions6" flags="self_collision">
upperarm_roll_left
- elbow_flex_left
- forearm_roll_left
- wrist_flex_left
- gripper_roll_left
+ torso
</group>
- <group name="collisions9" flags="self_collision">
- elbow_flex_right
- shoulder_pan_left
- shoulder_pitch_left
- upperarm_roll_left
+ <group name="collisions7" flags="self_collision">
elbow_flex_left
- forearm_roll_left
- wrist_flex_left
- gripper_roll_left
+ base
</group>
- <group name="collisions10" flags="self_collision">
- forearm_roll_right
- shoulder_pan_left
- shoulder_pitch_left
- upperarm_roll_left
- elbow_flex_left
+ <group name="collisions8" flags="self_collision">
forearm_roll_left
- wrist_flex_left
- gripper_roll_left
+ base
</group>
- <group name="collisions11" flags="self_collision">
- wrist_flex_right
- shoulder_pan_left
- shoulder_pitch_left
- upperarm_roll_left
- elbow_flex_left
- forearm_roll_left
+ <group name="collisions9" flags="self_collision">
wrist_flex_left
- gripper_roll_left
+ base
</group>
- <group name="collisions12" flags="self_collision">
- gripper_roll_right
- shoulder_pan_left
- shoulder_pitch_left
- upperarm_roll_left
- elbow_flex_left
- forearm_roll_left
- wrist_flex_left
+ <group name="collisions10" flags="self_collision">
gripper_roll_left
+ base
</group>
Modified: pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h
===================================================================
--- pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h 2008-08-26 20:31:49 UTC (rev 3634)
@@ -94,6 +94,8 @@
case dCylinderClass:
drawCylinder(geom);
break;
+ case dPlaneClass:
+ break;
default:
printf("Geometry class %d not yet implemented\n", cls);
break;
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-26 20:31:49 UTC (rev 3634)
@@ -267,7 +267,7 @@
void addSelfSeeBodies(void)
{
robot_desc::URDF::Group *ss = m_urdf->getGroup("self_see");
- if (ss && ss->hasFlag("mapping"))
+ if (ss && ss->hasFlag("collision"))
{
for (unsigned int i = 0 ; i < ss->linkNames.size() ; ++i)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
