Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2394] pkg/trunk
|
From: <is...@us...> - 2008-07-31 21:35:07
|
Revision: 2394
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2394&view=rev
Author: isucan
Date: 2008-07-31 21:35:13 +0000 (Thu, 31 Jul 2008)
Log Message:
-----------
a bit of code reorganization... need more testing
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/kinematic_planning/include/
pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h
Added: pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h 2008-07-31 21:35:13 UTC (rev 2394)
@@ -0,0 +1,119 @@
+/*********************************************************************
+* Software License Agreement (BSD License)
+*
+* Copyright (c) 2008, Willow Garage, Inc.
+* All rights reserved.
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*
+* * Redistributions of source code must retain the above copyright
+* notice, this list of conditions and the following disclaimer.
+* * Redistributions in binary form must reproduce the above
+* copyright notice, this list of conditions and the following
+* disclaimer in the documentation and/or other materials provided
+* with the distribution.
+* * Neither the name of the Willow Garage nor the names of its
+* contributors may be used to endorse or promote products derived
+* from this software without specific prior written permission.
+*
+* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+* POSSIBILITY OF SUCH DAMAGE.
+*********************************************************************/
+
+#include <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
+#include <planning_models/kinematic.h>
+
+/** This class configures an instance of SpaceInformationKinematic with data from a KinematicModel */
+class SpaceInformationXMLModel : public ompl::SpaceInformationKinematic
+{
+ public:
+ SpaceInformationXMLModel(planning_models::KinematicModel *kmodel, int groupID = -1, double divisions = 20.0) : SpaceInformationKinematic()
+ {
+ m_kmodel = kmodel;
+ m_groupID = groupID;
+ m_divisions = divisions;
+
+ m_stateDimension = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID].size() : m_kmodel->stateDimension;
+ m_stateComponent.resize(m_stateDimension);
+
+ for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ {
+ if (m_stateComponent[i].type == StateComponent::UNKNOWN)
+ m_stateComponent[i].type = StateComponent::NORMAL;
+ int p = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID][i] * 2 : i * 2;
+ m_stateComponent[i].minValue = m_kmodel->stateBounds[p ];
+ m_stateComponent[i].maxValue = m_kmodel->stateBounds[p + 1];
+ m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / m_divisions;
+
+ for (unsigned int j = 0 ; j < m_kmodel->floatingJoints.size() ; ++j)
+ if (m_kmodel->floatingJoints[j] == p)
+ {
+ m_floatingJoints.push_back(i);
+ m_stateComponent[i + 3].type = StateComponent::QUATERNION;
+ break;
+ }
+
+ for (unsigned int j = 0 ; j < m_kmodel->planarJoints.size() ; ++j)
+ if (m_kmodel->planarJoints[j] == p)
+ {
+ m_planarJoints.push_back(i);
+ break;
+ }
+ }
+ }
+
+ virtual ~SpaceInformationXMLModel(void)
+ {
+ }
+
+ void setPlanningVolume(double x0, double y0, double z0, double x1, double y1, double z1)
+ {
+ for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
+ {
+ int id = m_floatingJoints[i];
+ m_stateComponent[id ].minValue = x0;
+ m_stateComponent[id ].maxValue = x1;
+ m_stateComponent[id + 1].minValue = y0;
+ m_stateComponent[id + 1].maxValue = y1;
+ m_stateComponent[id + 2].minValue = z0;
+ m_stateComponent[id + 2].maxValue = z1;
+ for (int j = 0 ; j < 3 ; ++j)
+ m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
+ }
+ }
+
+ void setPlanningArea(double x0, double y0, double x1, double y1)
+ {
+ for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
+ {
+ int id = m_planarJoints[i];
+ m_stateComponent[id ].minValue = x0;
+ m_stateComponent[id ].maxValue = x1;
+ m_stateComponent[id + 1].minValue = y0;
+ m_stateComponent[id + 1].maxValue = y1;
+ for (int j = 0 ; j < 2 ; ++j)
+ m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
+ }
+ }
+
+ protected:
+
+ double m_divisions;
+ planning_models::KinematicModel *m_kmodel;
+ int m_groupID;
+ std::vector<int> m_floatingJoints;
+ std::vector<int> m_planarJoints;
+
+};
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-31 21:34:32 UTC (rev 2393)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-31 21:35:13 UTC (rev 2394)
@@ -88,8 +88,10 @@
#include <urdf/URDF.h>
#include <collision_space/environmentODE.h>
-#include <ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h>
+#include <ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h>
+#include "SpaceInformationXMLModel.h"
+
#include <vector>
#include <string>
#include <sstream>
@@ -120,7 +122,7 @@
m_collisionSpace->unlock();
// temp obstacle
- double sphere[6] = {0.8,0.2,0.4};
+ double sphere[6] = {2.8,0.2,0.4};
m_collisionSpace->addPointCloud(1, sphere, 0.15);
#ifdef DISPLAY_ODE_SPACES
@@ -215,10 +217,13 @@
return false;
/* set the workspace volume for planning */
- static_cast<SpaceInformationNode*>(p.si)->setPlanningVolume(req.volumeMin.x, req.volumeMin.y, req.volumeMin.z,
- req.volumeMax.x, req.volumeMax.y, req.volumeMax.z);
+ // only area or volume should go through... not sure what happens on really complicated models where
+ // we have both multiple planar and multiple floating joints...
+ static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningArea(req.volumeMin.x, req.volumeMin.y,
+ req.volumeMax.x, req.volumeMax.y);
+ static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningVolume(req.volumeMin.x, req.volumeMin.y, req.volumeMin.z,
+ req.volumeMax.x, req.volumeMax.y, req.volumeMax.z);
-
/* set the starting state */
ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
@@ -385,56 +390,7 @@
planning_models::KinematicModel *kmodel;
int groupID;
};
-
- class SpaceInformationNode : public ompl::SpaceInformationKinematic
- {
- public:
- SpaceInformationNode(Model *model) : SpaceInformationKinematic()
- {
- m_m = model;
- m_divisions = 20.0;
-
- m_stateDimension = m_m->groupID >= 0 ? m_m->kmodel->groupStateIndexList[m_m->groupID].size() : m_m->kmodel->stateDimension;
- m_stateComponent.resize(m_stateDimension);
-
- for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
- {
- m_stateComponent[i].type = StateComponent::NORMAL;
- unsigned int p = m_m->groupID >= 0 ? m_m->kmodel->groupStateIndexList[m_m->groupID][i] * 2 : i * 2;
- m_stateComponent[i].minValue = m_m->kmodel->stateBounds[p ];
- m_stateComponent[i].maxValue = m_m->kmodel->stateBounds[p + 1];
- m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / m_divisions;
- }
- }
-
- virtual ~SpaceInformationNode(void)
- {
- }
-
- void setPlanningVolume(double x0, double y0, double z0, double x1, double y1, double z1)
- {
- if (m_m->groupID < 0)
- for (unsigned int i = 0 ; i < m_m->kmodel->floatingJoints.size() ; ++i)
- {
- int id = m_m->kmodel->floatingJoints[i];
- m_stateComponent[id ].minValue = x0;
- m_stateComponent[id ].maxValue = x1;
- m_stateComponent[id + 1].minValue = y0;
- m_stateComponent[id + 1].maxValue = y1;
- m_stateComponent[id + 2].minValue = z0;
- m_stateComponent[id + 2].maxValue = z1;
- for (int j = 0 ; j < 3 ; ++j)
- m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
- }
- }
-
- protected:
-
- double m_divisions;
- Model *m_m;
-
- };
-
+
std_msgs::PointCloudFloat32 m_cloud;
collision_space::EnvironmentModel *m_collisionSpace;
std::map<std::string, Model*> m_models;
@@ -452,9 +408,9 @@
void createMotionPlanningInstances(Model* model)
{
Planner p;
- p.si = new SpaceInformationNode(model);
+ p.si = new SpaceInformationXMLModel(model->kmodel, model->groupID);
p.si->setStateValidFn(isStateValid, reinterpret_cast<void*>(model));
- p.mp = new ompl::LazyRRT(p.si);
+ p.mp = new ompl::RRT(p.si);
p.type = 0;
model->planners.push_back(p);
}
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-07-31 21:34:32 UTC (rev 2393)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h 2008-07-31 21:35:13 UTC (rev 2394)
@@ -161,9 +161,14 @@
struct StateComponent
{
+ StateComponent(void)
+ {
+ type = UNKNOWN;
+ }
+
enum
- { NORMAL, FIXED }
- type;
+ { UNKNOWN, NORMAL, QUATERNION, FIXED }
+ type;
double minValue;
double maxValue;
double resolution;
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-07-31 21:34:32 UTC (rev 2393)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-07-31 21:35:13 UTC (rev 2394)
@@ -48,7 +48,7 @@
robot_srvs::KinematicMotionPlan::request req;
robot_srvs::KinematicMotionPlan::response res;
- req.model_id = "pr2::base+rightArm";
+ req.model_id = "pr2::rightArm";
req.start_state.set_vals_size(32);
for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
@@ -57,14 +57,14 @@
for (unsigned int i = 18 ; i < 25 ; ++i)
req.start_state.vals[i] = 0.1;
- req.goal_state.set_vals_size(11);
+ 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.4;
req.allowed_time = 15.0;
req.volumeMin.x = -1.0; req.volumeMin.y = -1.0; req.volumeMin.z = -1.0;
- req.volumeMax.x = -1.0; req.volumeMax.y = -1.0; req.volumeMax.z = -1.0;
+ req.volumeMax.x = 1.0; req.volumeMax.y = 1.0; req.volumeMax.z = 1.0;
if (ros::service::call("plan_kinematic_path", req, res))
{
Modified: pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-31 21:34:32 UTC (rev 2393)
+++ pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-31 21:35:13 UTC (rev 2394)
@@ -107,7 +107,7 @@
/* relevant joint information */
enum
{
- UNKNOWN, FIXED, REVOLUTE, PRISMATIC, FLOATING
+ UNKNOWN, FIXED, REVOLUTE, PRISMATIC, PLANAR, FLOATING
} type;
double axis[3];
double anchor[3];
@@ -141,6 +141,9 @@
for (unsigned int i = 0 ; i < after.size() ; ++i)
delete after[i];
}
+
+ /** Name of the link */
+ std::string name;
/** Joint that connects this link to the parent link */
Joint *before;
@@ -209,6 +212,12 @@
planning, so the indices are provided here for
convenience. */
std::vector<int> floatingJoints;
+
+ /** The list of index values where planar joints
+ start. These joints need special attention in motion
+ planning, so the indices are provided here for
+ convenience. */
+ std::vector<int> planarJoints;
/* For each group, we have a list of index values in the
* computed state information that describe the components of
@@ -254,6 +263,9 @@
/** Cumulative list of floating joints */
std::vector<int> floatingJoints;
+
+ /** Cumulative list of planar joints */
+ std::vector<int> planarJoints;
/** Cumulative state dimension */
unsigned int stateDimension;
@@ -277,8 +289,13 @@
private:
+ /** Build the needed datastructure for a joint */
void buildChainJ(Robot *robot, Link *parent, Joint *joint, robot_desc::URDF::Link *urdfLink, robot_desc::URDF &model);
+
+ /** Build the needed datastructure for a link */
void buildChainL(Robot *robot, Joint *parent, Link *link, robot_desc::URDF::Link *urdfLink, robot_desc::URDF &model);
+
+ /** Construct the list of groups the model knows about (the ones marked with the 'plan' attribute) */
void constructGroupList(robot_desc::URDF &model);
};
Modified: pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-31 21:34:32 UTC (rev 2393)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-31 21:35:13 UTC (rev 2394)
@@ -34,6 +34,7 @@
#include <planning_models/kinematic.h>
#include <cstdio>
+#include <cmath>
void planning_models::KinematicModel::Robot::computeTransforms(const double *params, int groupID)
{
@@ -81,8 +82,13 @@
varTrans.setPosition(dx, dy, dz);
}
break;
+ case Joint::PLANAR:
+ varTrans.setPosition(params[0], params[1], 0.0);
+ varTrans.setAxisAngle(0.0, 0.0, 1.0, params[2]);
+ break;
case Joint::FLOATING:
varTrans.setPosition(params[0], params[1], params[2]);
+ varTrans.setQuaternion(params[3], params[4], params[5], params[6]);
break;
default:
break;
@@ -182,6 +188,10 @@
/* copy floating joints*/
for (unsigned int j = 0 ; j < m_robots[i]->floatingJoints.size() ; ++j)
floatingJoints.push_back(stateDimension + m_robots[i]->floatingJoints[j]);
+
+ /* copy planar joints*/
+ for (unsigned int j = 0 ; j < m_robots[i]->planarJoints.size() ; ++j)
+ planarJoints.push_back(stateDimension + m_robots[i]->planarJoints[j]);
/* copy group roots */
for (unsigned int j = 0 ; j < m_robots[i]->groupChainStart.size() ; ++j)
@@ -235,10 +245,18 @@
{
case robot_desc::URDF::Link::Joint::FLOATING:
joint->type = Joint::FLOATING;
- joint->usedParams = 3;
- robot->stateBounds.insert(robot->stateBounds.end(), 6, 0.0);
+ joint->usedParams = 7;
+ robot->stateBounds.insert(robot->stateBounds.end(), 14, 0.0);
robot->floatingJoints.push_back(robot->stateDimension);
break;
+ case robot_desc::URDF::Link::Joint::PLANAR:
+ joint->type = Joint::PLANAR;
+ joint->usedParams = 3;
+ robot->stateBounds.insert(robot->stateBounds.end(), 4, 0.0);
+ robot->stateBounds.push_back(-M_PI);
+ robot->stateBounds.push_back(M_PI);
+ robot->planarJoints.push_back(robot->stateDimension);
+ break;
case robot_desc::URDF::Link::Joint::FIXED:
joint->type = Joint::FIXED;
joint->usedParams = 0;
@@ -296,6 +314,7 @@
void planning_models::KinematicModel::buildChainL(Robot *robot, Joint *parent, Link* link, robot_desc::URDF::Link* urdfLink, robot_desc::URDF &model)
{
+ link->name = urdfLink->name;
link->before = parent;
robot->links.push_back(link);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
