Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2818] pkg/trunk
|
From: <is...@us...> - 2008-08-08 20:46:08
|
Revision: 2818
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2818&view=rev
Author: isucan
Date: 2008-08-08 20:46:14 +0000 (Fri, 08 Aug 2008)
Log Message:
-----------
motion planning in an automatically built environment works :)
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
pkg/trunk/robot_srvs/srv/KinematicMotionPlan.srv
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Added Paths:
-----------
pkg/trunk/world_models/world_3d_map/run.xml
Removed Paths:
-------------
pkg/trunk/world_models/world_3d_map/params.xml
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-08 20:44:59 UTC (rev 2817)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-08 20:46:14 UTC (rev 2818)
@@ -277,9 +277,13 @@
for (int j = 0 ; j < dim ; ++j)
res.path.states[i].vals[j] = path->states[i]->values[j];
}
+ res.distance = goal->getDifference();
}
else
+ {
res.path.set_states_size(0);
+ res.distance = -1.0;
+ }
/* cleanup */
p.si->clearGoal();
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-08 20:44:59 UTC (rev 2817)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-08 20:46:14 UTC (rev 2818)
@@ -35,6 +35,7 @@
#include <ros/node.h>
#include <ros/time.h>
#include <robot_srvs/KinematicMotionPlan.h>
+#include <robot_msgs/NamedKinematicPath.h>
#include <std_msgs/RobotBase2DOdom.h>
class PlanKinematicPath : public ros::node
@@ -43,6 +44,8 @@
PlanKinematicPath(void) : ros::node("plan_kinematic_path")
{
+ advertise<robot_msgs::NamedKinematicPath>("display_kinematic_path");
+
m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
m_haveBasePos = false;
subscribe("localizedpose", m_localizedPose, &PlanKinematicPath::localizedPoseCallback);
@@ -53,13 +56,12 @@
return m_haveBasePos;
}
- void runTest1(void)
+ void runTestBase(void)
{
robot_srvs::KinematicMotionPlan::request req;
- robot_srvs::KinematicMotionPlan::response res;
req.model_id = "pr2::base";
- req.threshold = 1e-6;
+ req.threshold = 0.01;
req.distance_metric = "L2Square";
req.start_state.set_vals_size(50);
@@ -69,28 +71,66 @@
req.goal_state.set_vals_size(3);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
{
- req.goal_state.vals[i] = m_basePos[i] + 1.0;
+ req.goal_state.vals[i] = m_basePos[i];
req.start_state.vals[i] = m_basePos[i];
}
+ req.goal_state.vals[0] += 4.5;
+
+ req.allowed_time = 5.0;
+
+ req.volumeMin.x = -5.0 + m_basePos[0]; req.volumeMin.y = -5.0 + m_basePos[1]; req.volumeMin.z = 0.0;
+ req.volumeMax.x = 5.0 + m_basePos[0]; req.volumeMax.y = 5.0 + m_basePos[1]; req.volumeMax.z = 0.0;
+
+ performCall(req);
+ }
+
+ void runTestLeftArm(void)
+ {
+ robot_srvs::KinematicMotionPlan::request req;
+
+ req.model_id = "pr2::leftArm";
+ req.threshold = 0.01;
+ req.distance_metric = "L2Square";
+
+ req.start_state.set_vals_size(50);
+ for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
+ req.start_state.vals[i] = 0.0;
+
+ req.goal_state.set_vals_size(7);
+ for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
+ req.goal_state.vals[i] = 0.1;
+
req.allowed_time = 10.0;
req.volumeMin.x = -5.0 + m_basePos[0]; req.volumeMin.y = -5.0 + m_basePos[1]; req.volumeMin.z = 0.0;
req.volumeMax.x = 5.0 + m_basePos[0]; req.volumeMax.y = 5.0 + m_basePos[1]; req.volumeMax.z = 0.0;
+ performCall(req);
+ }
+
+ void performCall(robot_srvs::KinematicMotionPlan::request &req)
+ {
+ robot_srvs::KinematicMotionPlan::response res;
+ robot_msgs::NamedKinematicPath dpath;
+
if (ros::service::call("plan_kinematic_path", req, res))
{
unsigned int nstates = res.path.get_states_size();
- printf("Obtained solution path with %u states\n", nstates);
+ printf("Obtained %ssolution path with %u states, distance to goal = %f\n",
+ res.distance > req.threshold ? "approximate " : "", nstates, res.distance);
for (unsigned int i = 0 ; i < nstates ; ++i)
{
for (unsigned int j = 0 ; j < res.path.states[i].get_vals_size() ; ++j)
printf("%f ", res.path.states[i].vals[j]);
printf("\n");
- }
+ }
+ dpath.name = req.model_id;
+ dpath.path = res.path;
+ publish("display_kinematic_path", dpath);
}
else
- fprintf(stderr, "Service 'plan_kinematic_path' failed\n");
+ fprintf(stderr, "Service 'plan_kinematic_path' failed\n");
}
private:
@@ -119,9 +159,10 @@
ros::Duration dur(0.1);
while (!plan.haveBasePos())
dur.sleep();
+
+ // plan.runTestLeftArm();
+ plan.runTestBase();
- plan.runTest1();
-
plan.shutdown();
return 0;
Modified: pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-08 20:44:59 UTC (rev 2817)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-08 20:46:14 UTC (rev 2818)
@@ -108,10 +108,12 @@
subscribe("display_kinematic_path", m_displayPath, &PlanningWorldViewer::displayPathCallback);
m_collisionSpace = new collision_space::EnvironmentModelODE();
+ m_collisionSpace->setSelfCollision(false);
m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
m_follow = true;
m_displayRobot = true;
m_displayObstacles = true;
+ m_checkCollision = false;
m_collisionSpace->lock();
loadRobotDescriptions();
@@ -172,6 +174,11 @@
m_collisionSpace->lock();
m_collisionSpace->getModel(0)->computeTransforms(m_basePos, group);
m_collisionSpace->updateRobotModel(0);
+ if (m_checkCollision)
+ {
+ ros::Time startTime = ros::Time::now();
+ printf("Collision: %d [%f s]\n", m_collisionSpace->isCollision(0), (ros::Time::now() - startTime).to_double());
+ }
m_collisionSpace->unlock();
}
}
@@ -257,13 +264,25 @@
/* add the model to the collision space */
unsigned int cid = m_collisionSpace->addRobotModel(kmodel);
+ defaultPosition(kmodel, cid);
+ }
+
+ void defaultPosition(planning_models::KinematicModel *kmodel = NULL, unsigned int cid = 0)
+ {
+ if (!kmodel)
+ {
+ if (m_collisionSpace->getModelCount() == 1)
+ kmodel = m_collisionSpace->getModel(0);
+ else
+ return;
+ }
+
double defaultPose[kmodel->stateDimension];
for (unsigned int i = 0 ; i < kmodel->stateDimension ; ++i)
defaultPose[i] = 0.0;
kmodel->computeTransforms(defaultPose);
- m_collisionSpace->updateRobotModel(cid);
-
+ m_collisionSpace->updateRobotModel(cid);
}
unsigned int getRobotCount(void) const
@@ -281,6 +300,16 @@
m_follow = follow;
}
+ bool getCheckCollision(void) const
+ {
+ return m_checkCollision;
+ }
+
+ void setCheckCollision(bool collision)
+ {
+ m_checkCollision = collision;
+ }
+
bool getDisplayRobot(void) const
{
return m_displayRobot;
@@ -334,7 +363,8 @@
bool m_follow;
bool m_displayRobot;
bool m_displayObstacles;
-
+ bool m_checkCollision;
+
};
static PlanningWorldViewer *viewer = NULL;
@@ -365,6 +395,10 @@
case 'F':
viewer->setFollow(!viewer->getFollow());
break;
+ case 'c':
+ case 'C':
+ viewer->setCheckCollision(!viewer->getCheckCollision());
+ break;
case 'R':
viewer->setDisplayRobot(!viewer->getDisplayRobot());
@@ -372,6 +406,9 @@
case 'O':
viewer->setDisplayObstacles(!viewer->getDisplayObstacles());
break;
+ case 'D':
+ viewer->defaultPosition();
+ break;
default:
break;
}
Modified: pkg/trunk/robot_srvs/srv/KinematicMotionPlan.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicMotionPlan.srv 2008-08-08 20:44:59 UTC (rev 2817)
+++ pkg/trunk/robot_srvs/srv/KinematicMotionPlan.srv 2008-08-08 20:46:14 UTC (rev 2818)
@@ -59,5 +59,10 @@
# includes start and goal states) to define the motions for the
# robot. If more intermediate states are needed, linear interpolation
# is to be assumed.
+robot_msgs/KinematicPath path
-robot_msgs/KinematicPath path
+
+# The threshold that was actually achieved. If the planner did not have enough time,
+# it may return a distance larger than the desired threshold. The user may choose to
+# discard the path
+float64 distance
Deleted: pkg/trunk/world_models/world_3d_map/params.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/params.xml 2008-08-08 20:44:59 UTC (rev 2817)
+++ pkg/trunk/world_models/world_3d_map/params.xml 2008-08-08 20:46:14 UTC (rev 2818)
@@ -1,7 +0,0 @@
-<params>
- <double name="world_3d_map/max_publish_frequency">0.3</double> <!-- Hz -->
- <double name="world_3d_map/retain_pointcloud_duration">60</double> <!-- seconds -->
- <double name="world_3d_map/retain_pointcloud_fraction">0.02</double> <!-- percentage (between 0 and 1) -->
- <int name="world_3d_map/verbosity_level">1</int> <!-- integer value -->
- <string name="world_3d_map/robot_model">robotdesc/pr2</string> <!-- string value -->
-</params>
Added: pkg/trunk/world_models/world_3d_map/run.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/run.xml (rev 0)
+++ pkg/trunk/world_models/world_3d_map/run.xml 2008-08-08 20:46:14 UTC (rev 2818)
@@ -0,0 +1,11 @@
+<launch>
+
+ <param name="world_3d_map/max_publish_frequency" type="double" value="0.3" /> <!-- Hz -->
+ <param name="world_3d_map/retain_pointcloud_duration" type="double" value="60.0" /> <!-- seconds -->
+ <param name="world_3d_map/retain_pointcloud_fraction" type="double" value="0.02" /> <!-- percentage (between 0 and 1) -->
+ <param name="world_3d_map/verbosity_level" type="int" value="1" /> <!-- integer value -->
+ <param name="world_3d_map/robot_model" type="str" value="robotdesc/pr2" /> <!-- string value -->
+
+ <node pkg="world_3d_map" type="world_3d_map" respawn="false" />
+
+</launch>
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-08 20:44:59 UTC (rev 2817)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-08 20:46:14 UTC (rev 2818)
@@ -310,7 +310,7 @@
if (m_shouldPublish)
{
m_worldDataMutex.lock();
- if (m_active)
+ if (m_active && m_currentWorld.size() > 0)
{
PointCloudFloat32 toPublish;
toPublish.header = m_currentWorld.back()->header;
@@ -378,7 +378,7 @@
}
rp.body->setDimensions(link->geom->size);
- rp.body->setScale(1.4);
+ rp.body->setScale(1.5);
m_selfSeeParts.push_back(rp);
}
@@ -469,7 +469,7 @@
double y = cloud.pts[k].y;
double z = cloud.pts[k].z;
- if (z > 1e-6)
+ if (z > 0.01)
{
bool keep = true;
for (int i = m_selfSeeParts.size() - 1 ; keep && i >= 0 ; --i)
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
