Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[3220] pkg/trunk
|
From: <is...@us...> - 2008-08-18 21:48:43
|
Revision: 3220
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3220&view=rev
Author: isucan
Date: 2008-08-18 21:48:42 +0000 (Mon, 18 Aug 2008)
Log Message:
-----------
added path interpolation option for motion plans + fixed call parameters
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.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/KinematicPlanLinkPosition.srv
pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-18 21:48:42 UTC (rev 3220)
@@ -197,6 +197,8 @@
p.si->smoother->smoothVertices(path);
double tsmooth = (ros::Time::now() - startTime).to_double();
printf("Smoother spent %f seconds (%f seconds in total)\n", tsmooth, tsmooth + tsolve);
+ if (req.interpolate)
+ p.si->interpolatePath(path);
}
m_collisionSpace->unlock();
Modified: pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h 2008-08-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/motion_planning/ompl/code/ompl/base/SpaceInformation.h 2008-08-18 21:48:42 UTC (rev 3220)
@@ -85,11 +85,11 @@
class StateValidityChecker
{
public:
- /** Destructor */
- virtual ~StateValidityChecker(void){
-
- }
-
+ /** Destructor */
+ virtual ~StateValidityChecker(void)
+ {
+ }
+
/** Return true if the state is valid */
virtual bool operator()(const State_t state) = 0;
};
@@ -101,10 +101,10 @@
class StateDistanceEvaluator
{
public:
- /** Destructor */
- virtual ~StateDistanceEvaluator(void){
-
- }
+ /** Destructor */
+ virtual ~StateDistanceEvaluator(void)
+ {
+ }
/** Return true if the state is valid */
virtual double operator()(const State_t state1, const State_t state2) = 0;
};
@@ -302,9 +302,9 @@
}
/** Clear the goal. Memory is freed. */
- void clearGoal(bool free = true)
+ void clearGoal(void)
{
- if (free && m_goal)
+ if (m_goal)
delete m_goal;
m_goal = NULL;
}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2008-08-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/src/SpaceInformationKinematic.cpp 2008-08-18 21:48:42 UTC (rev 3220)
@@ -189,11 +189,10 @@
state->values[k] = s1->values[k] + (double)j * step[k];
newStates.push_back(state);
}
-
- newStates.push_back(s2);
}
+ newStates.push_back(path->states[n1]);
- path->states.swap(newStates);
+ path->states.swap(newStates);
}
void ompl::SpaceInformationKinematic::printSettings(FILE *out) const
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-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-18 21:48:42 UTC (rev 3220)
@@ -56,6 +56,16 @@
return m_haveBasePos;
}
+ void initialState(robot_msgs::KinematicState &state)
+ {
+ state.set_vals_size(45);
+ for (unsigned int i = 0 ; i < state.get_vals_size() ; ++i)
+ state.vals[i] = 0.0;
+ for (unsigned int i = 0 ; i < 3 ; ++i)
+ state.vals[i] = m_basePos[i];
+ }
+
+
void runTestBase(void)
{
robot_srvs::KinematicPlanState::request req;
@@ -63,20 +73,15 @@
req.model_id = "pr2::base";
req.threshold = 0.01;
req.distance_metric = "L2Square";
+ req.interpolate = 1;
- req.start_state.set_vals_size(45);
- for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
- req.start_state.vals[i] = 0.0;
+ initialState(req.start_state);
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];
req.start_state.vals[i] = m_basePos[i];
- }
req.goal_state.vals[0] += 3.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;
@@ -92,11 +97,10 @@
req.model_id = "pr2::leftArm";
req.threshold = 0.01;
req.distance_metric = "L2Square";
+ req.interpolate = 1;
+
+ initialState(req.start_state);
- req.start_state.set_vals_size(45);
- 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.0;
@@ -118,11 +122,10 @@
req.model_id = "pr2::rightArm";
req.threshold = 0.01;
req.distance_metric = "L2Square";
+ req.interpolate = 1;
+
+ initialState(req.start_state);
- req.start_state.set_vals_size(45);
- 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.0;
@@ -187,9 +190,23 @@
while (!plan.haveBasePos())
dur.sleep();
- // plan.runTestLeftArm();
- // plan.runTestBase();
- plan.runTestRightArm();
+ char test = (argc < 2) ? 'b' : argv[1][0];
+
+ switch (test)
+ {
+ case 'l':
+ plan.runTestLeftArm();
+ break;
+ case 'b':
+ plan.runTestBase();
+ break;
+ case 'r':
+ plan.runTestRightArm();
+ break;
+ default:
+ printf("Unknown test\n");
+ break;
+ }
plan.shutdown();
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-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-18 21:48:42 UTC (rev 3220)
@@ -103,7 +103,7 @@
PlanningWorldViewer(const std::string &robot_model) : ros::node("planning_world_viewer"),
planning_node_util::NodeODECollisionModel(dynamic_cast<ros::node*>(this), robot_model)
{
- subscribe("display_kinematic_path", m_displayPath, &PlanningWorldViewer::displayPathCallback);
+ subscribe("display_kinematic_path", m_displayPath, &PlanningWorldViewer::displayPathCallback, 1);
m_follow = true;
m_displayRobot = true;
@@ -130,6 +130,20 @@
m_collisionSpace->unlock();
}
+ bool checkCollision(void)
+ {
+ bool result = false;
+ if (m_checkCollision && m_collisionSpace && m_collisionSpace->getModelCount() == 1)
+ {
+ m_collisionSpace->lock();
+ ros::Time startTime = ros::Time::now();
+ result = m_collisionSpace->isCollision(0);
+ printf("Collision: %d [%f s]\n", result, (ros::Time::now() - startTime).to_double());
+ m_collisionSpace->unlock();
+ }
+ return result;
+ }
+
void baseUpdate(void)
{
planning_node_util::NodeODECollisionModel::baseUpdate();
@@ -140,12 +154,8 @@
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();
+ checkCollision();
}
}
@@ -167,13 +177,14 @@
{
bool follow = m_follow;
m_follow = false;
- ros::Duration sleepTime(0.2);
+ ros::Duration sleepTime(0.4);
int groupID = m_kmodel->getGroupID(m_displayPath.name);
for (unsigned int i = 0 ; i < m_displayPath.path.get_states_size() ; ++i)
{
m_kmodel->computeTransforms(m_displayPath.path.states[i].vals, groupID);
m_collisionSpace->updateRobotModel(0);
+ checkCollision();
sleepTime.sleep();
}
m_follow = follow;
@@ -240,8 +251,6 @@
void display(void)
{
- // printf("disp\n");
-
m_displayLock.lock();
m_spaces.displaySpaces();
m_displayLock.unlock();
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv 2008-08-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv 2008-08-18 21:48:42 UTC (rev 3220)
@@ -25,7 +25,11 @@
# No state in the produced motion plan will violate these constraints
robot_msgs/KinematicConstraint[] constraints
+# Boolean flag: if true, the returned path will contain interpolated
+# states as well
+byte interpolate
+
# The maximum amount of time the motion planner is allowed to plan for
float64 allowed_time
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2008-08-18 20:57:28 UTC (rev 3219)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2008-08-18 21:48:42 UTC (rev 3220)
@@ -28,6 +28,11 @@
# No state in the produced motion plan will violate these constraints
robot_msgs/KinematicConstraint[] constraints
+# Boolean flag: if true, the returned path will contain interpolated
+# states as well
+
+byte interpolate
+
# The maximum amount of time the motion planner is allowed to plan for
float64 allowed_time
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
