Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3480] pkg/trunk
|
From: <is...@us...> - 2008-08-22 17:35:53
|
Revision: 3480
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3480&view=rev
Author: isucan
Date: 2008-08-22 17:36:01 +0000 (Fri, 22 Aug 2008)
Log Message:
-----------
computed some good parameters for rrt and lazy rrt, automatically loading them from the parameter server
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-08-22 17:36:01 UTC (rev 3480)
@@ -8,5 +8,6 @@
<depend package="planning_node_util" />
<depend package="ompl" />
<depend package="profiling_utils" />
+ <depend package="string_utils" />
<depend package="gtest" />
</package>
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-22 17:36:01 UTC (rev 3480)
@@ -98,6 +98,7 @@
#include "SpaceInformationXMLModel.h"
#include <profiling_utils/profiler.h>
+#include <string_utils/string_utils.h>
#include <iostream>
#include <sstream>
@@ -115,11 +116,6 @@
robot_model)
{
advertise_service("plan_kinematic_path_state", &KinematicPlanning::plan);
-
-
- double sphere[3] = {0.7, 0.5, 0.7 };
- m_collisionSpace->addPointCloud(1, sphere, 0.2);
-
}
~KinematicPlanning(void)
@@ -285,10 +281,11 @@
model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
+ model->groupName = m_kmodel->name;
createMotionPlanningInstances(model);
/* remember the model by the robot's name */
- m_models[m_urdf->getRobotName()] = model;
+ m_models[model->groupName] = model;
printf("=======================================\n");
m_kmodel->printModelInfo();
@@ -305,8 +302,9 @@
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
model->groupID = m_kmodel->getGroupID(groups[i]);
+ model->groupName = groups[i];
createMotionPlanningInstances(model);
- m_models[groups[i]] = model;
+ m_models[model->groupName] = model;
}
}
@@ -352,7 +350,8 @@
collision_space::EnvironmentModel *collisionSpace;
unsigned int collisionSpaceID;
planning_models::KinematicModel *kmodel;
- int groupID;
+ std::string groupName;
+ int groupID;
};
class StateValidityPredicate : public ompl::SpaceInformation::StateValidityChecker
@@ -377,7 +376,13 @@
Model *m_model;
};
-
+ robot_desc::URDF::Group* getURDFGroup(const std::string &gname)
+ {
+ std::string urdfGroup = gname;
+ urdfGroup.erase(0, urdfGroup.find_last_of(":") + 1);
+ return m_urdf->getGroup(urdfGroup);
+ }
+
void createMotionPlanningInstances(Model* model)
{
addRRTInstance(model);
@@ -386,29 +391,81 @@
void addRRTInstance(Model *model)
{
Planner p;
+ std::cout << "Adding RRT instance for motion planning: " << model->groupName << std::endl;
+
p.si = new SpaceInformationXMLModel(model->kmodel, model->groupID);
p.svc = new StateValidityPredicate(model);
p.si->setStateValidityChecker(p.svc);
+
p.smoother = new ompl::PathSmootherKinematic(p.si);
- p.smoother->setMaxSteps(20);
- p.mp = new ompl::RRT(p.si);
+ p.smoother->setMaxSteps(20);
+
+ ompl::RRT_t rrt = new ompl::RRT(p.si);
+ p.mp = rrt;
+
+ robot_desc::URDF::Group *group = getURDFGroup(model->groupName);
+ if (group)
+ {
+ const robot_desc::URDF::Map &data = group->data;
+ std::map<std::string, std::string> info = data.getMapTagValues("plan", "RRT");
+
+ if (info.find("range") != info.end())
+ {
+ double range = string_utils::fromString<double>(info["range"]);
+ rrt->setRange(range);
+ std::cout << "Range is set to " << range << std::endl;
+ }
+
+ if (info.find("goal_bias") != info.end())
+ {
+ double bias = string_utils::fromString<double>(info["goal_bias"]);
+ rrt->setGoalBias(bias);
+ std::cout << "Goal bias is set to " << bias << std::endl;
+ }
+ }
+
p.si->setup();
model->planners.push_back(p);
- std::cout << "Added RRT instance for motion planning" << std::endl;
}
void addLazyRRTInstance(Model *model)
{
Planner p;
+ std::cout << "Added LazyRRT instance for motion planning: " << model->groupName << std::endl;
+
p.si = new SpaceInformationXMLModel(model->kmodel, model->groupID);
p.svc = new StateValidityPredicate(model);
p.si->setStateValidityChecker(p.svc);
+
p.smoother = new ompl::PathSmootherKinematic(p.si);
- p.smoother->setMaxSteps(20);
- p.mp = new ompl::LazyRRT(p.si);
+ p.smoother->setMaxSteps(20);
+
+ ompl::LazyRRT_t lrrt = new ompl::LazyRRT(p.si);
+ p.mp = lrrt;
+
+ robot_desc::URDF::Group *group = getURDFGroup(model->groupName);
+ if (group)
+ {
+ const robot_desc::URDF::Map &data = group->data;
+ std::map<std::string, std::string> info = data.getMapTagValues("plan", "LazyRRT");
+
+ if (info.find("range") != info.end())
+ {
+ double range = string_utils::fromString<double>(info["range"]);
+ lrrt->setRange(range);
+ std::cout << "Range is set to " << range << std::endl;
+ }
+
+ if (info.find("goal_bias") != info.end())
+ {
+ double bias = string_utils::fromString<double>(info["goal_bias"]);
+ lrrt->setGoalBias(bias);
+ std::cout << "Goal bias is set to " << bias << std::endl;
+ }
+ }
+
p.si->setup();
model->planners.push_back(p);
- std::cout << "Added LazyRRT instance for motion planning" << std::endl;
}
std::map<std::string, Model*> m_models;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h 2008-08-22 17:36:01 UTC (rev 3480)
@@ -56,7 +56,7 @@
m_nn.setDataParameter(reinterpret_cast<void*>(dynamic_cast<SpaceInformationKinematic_t>(m_si)));
random_utils::random_init(&m_rngState);
m_goalBias = 0.05;
- m_rho = 0.5;
+ m_rho = 0.1;
}
virtual ~LazyRRT(void)
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h 2008-08-22 17:36:01 UTC (rev 3480)
@@ -55,7 +55,7 @@
m_nn.setDataParameter(reinterpret_cast<void*>(dynamic_cast<SpaceInformationKinematic_t>(m_si)));
random_utils::random_init(&m_rngState);
m_goalBias = 0.05;
- m_rho = 0.5;
+ m_rho = 0.1;
}
virtual ~RRT(void)
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-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-22 17:36:01 UTC (rev 3480)
@@ -104,7 +104,7 @@
req.threshold = 0.01;
req.distance_metric = "L2Square";
req.interpolate = 1;
- req.times = 5;
+ req.times = 100;
initialState(req.start_state);
@@ -130,7 +130,7 @@
req.threshold = 0.01;
req.distance_metric = "L2Square";
req.interpolate = 1;
- req.times = 1;
+ req.times = 100;
initialState(req.start_state);
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2008-08-22 17:36:01 UTC (rev 3480)
@@ -344,9 +344,6 @@
void computeTransforms(const double *params, int groupID = -1);
- /** The name of the robot */
- std::string name;
-
/** The model that owns this robot */
KinematicModel *owner;
@@ -421,6 +418,9 @@
void printModelInfo(std::ostream &out = std::cout) const;
void printLinkPoses(std::ostream &out = std::cout) const;
+ /** The name of the model */
+ std::string name;
+
/** A transform that is applied to the entire model */
libTF::Pose3D rootTransform;
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-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp 2008-08-22 17:36:01 UTC (rev 3480)
@@ -264,7 +264,8 @@
m_built = true;
m_ignoreSensors = ignoreSensors;
-
+ name = model.getRobotName();
+
/* construct a map for the available groups */
constructGroupList(model);
groupStateIndexList.resize(m_groups.size());
@@ -276,7 +277,6 @@
if (link->canSense() && m_ignoreSensors)
continue;
Robot *rb = new Robot(this);
- rb->name = model.getRobotName();
rb->groupStateIndexList.resize(m_groups.size());
rb->groupChainStart.resize(m_groups.size());
rb->chain = createJoint(link);
@@ -373,7 +373,7 @@
for (unsigned int k = 0 ; k < urdfLink->groups.size() ; ++k)
if (urdfLink->groups[k]->isRoot(urdfLink))
{
- std::string gname = robot->name + "::" + urdfLink->groups[k]->name;
+ std::string gname = name + "::" + urdfLink->groups[k]->name;
if (m_groupsMap.find(gname) != m_groupsMap.end())
robot->groupChainStart[m_groupsMap[gname]].push_back(joint);
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-22 17:36:01 UTC (rev 3480)
@@ -38,6 +38,14 @@
forearm_roll_left
wrist_flex_left
gripper_roll_left
+
+ <map name="RRT" flag="plan">
+ <elem key="range" value="0.75" />
+ </map>
+
+ <map name="LazyRRT" flag="plan">
+ <elem key="range" value="0.75" />
+ </map>
</group>
<group name="rightArm" flags="plan kinematic">
@@ -50,9 +58,13 @@
wrist_flex_right
gripper_roll_right
- <map name="rrt" flag="plan">
- <elem key="rho" value="0.5" />
+ <map name="RRT" flag="plan">
+ <elem key="range" value="0.75" />
</map>
+
+ <map name="LazyRRT" flag="plan">
+ <elem key="range" value="0.75" />
+ </map>
</group>
<group name="base+leftArm" flags="plan">
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
