Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2023] pkg/trunk
|
From: <is...@us...> - 2008-07-24 00:38:00
|
Revision: 2023
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2023&view=rev
Author: isucan
Date: 2008-07-24 00:38:09 +0000 (Thu, 24 Jul 2008)
Log Message:
-----------
setting the extent of the workspace to plan in
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.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
pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-24 00:32:12 UTC (rev 2022)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-24 00:38:09 UTC (rev 2023)
@@ -173,11 +173,9 @@
tf.setPosition(req.transform.xt, req.transform.yt, req.transform.zt);
tf.setQuaternion(req.transform.xr, req.transform.yr, req.transform.zr, req.transform.w);
-
- // set the 3D space bounding box for planning.
- // if not specified in the request, infer it based on start + goal positions
- // need to know where floating joints are and set these there. also update resolution
-
+ static_cast<SpaceInformationNode*>(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);
for (int i = 0 ; i < dim ; ++i)
@@ -209,11 +207,11 @@
}
/* cleanup */
- p.si->clearGoal();
+ p.si->clearGoal();
p.si->clearStartStates(true);
p.mp->clear();
- return true;
+ return true;
}
void addRobotDescriptionFromFile(const char *filename)
@@ -295,6 +293,9 @@
public:
SpaceInformationNode(planning_models::KinematicModel *km) : SpaceInformationKinematic()
{
+ m_km = km;
+ m_divisions = 20.0;
+
m_stateDimension = km->stateDimension;
m_stateComponent.resize(m_stateDimension);
@@ -303,15 +304,35 @@
m_stateComponent[i].type = StateComponent::NORMAL;
m_stateComponent[i].minValue = km->stateBounds[i*2 ];
m_stateComponent[i].maxValue = km->stateBounds[i*2 + 1];
- m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / 20.0;
- if (m_stateComponent[i].resolution == 0.0)
- m_stateComponent[i].resolution = 0.1; // this happens for floating joints
+ 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)
+ {
+ for (unsigned int i = 0 ; i < m_km->floatingJoints.size() ; ++i)
+ {
+ int id = m_km->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;
+ planning_models::KinematicModel *m_km;
+
};
std_msgs::PointCloudFloat32 m_cloud;
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-24 00:32:12 UTC (rev 2022)
+++ pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-24 00:38:09 UTC (rev 2023)
@@ -197,6 +197,12 @@
* set to 0, the parameter is unbounded. */
std::vector<double> stateBounds;
+ /** The list of index values where floating joints
+ start. These joints need special attention in motion
+ planning, so the indices are provided here for
+ convenience. */
+ std::vector<int> floatingJoints;
+
/** The model that owns this robot */
KinematicModel *owner;
@@ -235,6 +241,9 @@
/** Cumulative state bounds */
std::vector<double> stateBounds;
+ /** Cumulative list of floating joints */
+ std::vector<int> floatingJoints;
+
protected:
std::vector<Robot*> m_robots;
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-24 00:32:12 UTC (rev 2022)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-24 00:38:09 UTC (rev 2023)
@@ -122,9 +122,11 @@
for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
{
+ stateBounds.insert(stateBounds.end(), m_robots[i]->stateBounds.begin(), m_robots[i]->stateBounds.end());
+ for (unsigned int j = 0 ; j < m_robots[i]->floatingJoints.size() ; ++j)
+ floatingJoints.push_back(stateDimension + m_robots[i]->floatingJoints[j]);
stateDimension += m_robots[i]->stateDimension;
- stateBounds.insert(stateBounds.end(), m_robots[i]->stateBounds.begin(), m_robots[i]->stateBounds.end());
- }
+ }
}
unsigned int planning_models::KinematicModel::getRobotCount(void) const
@@ -158,6 +160,7 @@
joint->type = Joint::FLOATING;
joint->usedParamEnd = joint->usedParamStart + 3;
robot->stateBounds.insert(robot->stateBounds.end(), 6, 0.0);
+ robot->floatingJoints.push_back(joint->usedParamStart);
break;
case robot_desc::URDF::Link::Joint::FIXED:
joint->type = Joint::FIXED;
Modified: pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv
===================================================================
--- pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv 2008-07-24 00:32:12 UTC (rev 2022)
+++ pkg/trunk/services/robot_srvs/srv/KinematicMotionPlan.srv 2008-07-24 00:38:09 UTC (rev 2023)
@@ -3,6 +3,7 @@
robot_msgs/KinematicState goal_state
float64 allowed_time
std_msgs/TransformQuaternion transform
-std_msgs/Point3DFloat64 volume
+std_msgs/Point3DFloat64 volumeMin
+std_msgs/Point3DFloat64 volumeMax
---
robot_msgs/KinematicPath path
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
