Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[1794] pkg/trunk
|
From: <is...@us...> - 2008-07-18 23:45:33
|
Revision: 1794
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=1794&view=rev
Author: isucan
Date: 2008-07-18 23:45:42 +0000 (Fri, 18 Jul 2008)
Log Message:
-----------
more incremental updates
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
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-18 23:27:55 UTC (rev 1793)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-18 23:45:42 UTC (rev 1794)
@@ -139,7 +139,7 @@
{
// const int dim = req.start_state.vals_size;
// ompl::SpaceInformationKinematic::GoalStateKinematic_t goal = new ompl::SpaceInformationKinematic::GoalStateKinematic(m_si);
- // m_si->setGoal(goal);
+ //m_si->setGoal(goal);
/*
std::vector<double*> path;
@@ -189,16 +189,7 @@
model->collisionSpaceID = m_collisionSpace->addRobotModel(*file, groups[i].c_str());
m_models[name + "::" + groups[i]] = model;
createMotionPlanningInstances(model);
-
- // pass it to the collision space to create one more kinematic model
- // find the state dimension & bounding box, set up the space information
- // create a motion planner for this space info
-
-
- // todo: kinematic model should print what each state dimension means
-
- }
-
+ }
}
private:
@@ -232,6 +223,30 @@
unsigned int collisionSpaceID;
};
+ class SpaceInformationNode : public ompl::SpaceInformationKinematic
+ {
+ public:
+ SpaceInformationNode(planning_models::KinematicModel *km) : SpaceInformationKinematic()
+ {
+ m_stateDimension = km->stateDimension;
+ m_stateComponent.resize(m_stateDimension);
+
+ for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ {
+ 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
+ }
+ }
+
+ virtual ~SpaceInformationNode(void)
+ {
+ }
+ };
+
std_msgs::PointCloudFloat32 m_cloud;
collision_space::EnvironmentModel *m_collisionSpace;
std::map<std::string, Model*> m_models;
@@ -240,24 +255,19 @@
static bool isStateValid(const ompl::SpaceInformationKinematic::StateKinematic_t state, void *data)
{
Model *model = reinterpret_cast<Model*>(data);
-
- return false;
+ return model->collisionSpace->isCollision(model->collisionSpaceID);
}
void createMotionPlanningInstances(Model* model)
{
+ model->collisionSpace = m_collisionSpace;
+
Planner p;
- p.si = new ompl::SpaceInformationKinematic();
+ p.si = new SpaceInformationNode(m_collisionSpace->models[model->collisionSpaceID]);
+ p.si->setStateValidFn(isStateValid, reinterpret_cast<void*>(model));
p.mp = new ompl::RRT(p.si);
- p.type = 0;
-
+ p.type = 0;
model->planners.push_back(p);
- model->collisionSpace = m_collisionSpace;
-
- planning_models::KinematicModel *km = m_collisionSpace->models[model->collisionSpaceID];
- p.si->setStateValidFn(isStateValid, reinterpret_cast<void*>(model));
-
-
}
};
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-18 23:27:55 UTC (rev 1793)
+++ pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-18 23:45:42 UTC (rev 1794)
@@ -204,7 +204,8 @@
KinematicModel(void)
{
- m_verbose = false;
+ stateDimension = 0;
+ m_verbose = true;
}
virtual ~KinematicModel(void)
@@ -215,14 +216,23 @@
virtual void build(robot_desc::URDF &model, const char *group = NULL);
+ void setVerbose(bool verbose);
+
unsigned int getRobotCount(void) const;
Robot* getRobot(unsigned int index) const;
+ void computeTransforms(const double *params);
+
+ /** Cumulative state dimension */
+ unsigned int stateDimension;
+ /** Cumulative state bounds */
+ std::vector<double> stateBounds;
+
protected:
std::vector<Robot*> m_robots;
bool m_verbose;
-
+
private:
void buildChain(Robot *robot, Link *parent, Joint *joint, robot_desc::URDF::Link *urdfLink);
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-18 23:27:55 UTC (rev 1793)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-18 23:45:42 UTC (rev 1794)
@@ -40,6 +40,12 @@
chain->computeTransform(params);
}
+void planning_models::KinematicModel::computeTransforms(const double *params)
+{
+ for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
+ m_robots[i]->chain->computeTransform(params + i);
+}
+
// we can optimize things here... (when we use identity transforms, for example)
void planning_models::KinematicModel::Joint::computeTransform(const double *params)
{
@@ -82,6 +88,11 @@
globalTrans.multiplyPose(constGeomTrans);
}
+void planning_models::KinematicModel::setVerbose(bool verbose)
+{
+ m_verbose = verbose;
+}
+
void planning_models::KinematicModel::build(robot_desc::URDF &model, const char *group)
{
if (group)
@@ -108,6 +119,12 @@
m_robots.push_back(rb);
}
}
+
+ for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
+ {
+ 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
@@ -164,6 +181,15 @@
break;
}
joint->active = joint->usedParamEnd > joint->usedParamStart;
+ if (m_verbose && joint->usedParamEnd > joint->usedParamStart)
+ {
+ printf("Joint '%s' connects link '%s' to link '%s' and uses state coordinates: ",
+ urdfLink->joint->name.c_str(), urdfLink->parentName.c_str(), urdfLink->name.c_str());
+ for (unsigned int i = joint->usedParamStart ; i < joint->usedParamEnd ; ++i)
+ printf("%d ", i);
+ printf("\n");
+ }
+
robot->stateDimension = joint->usedParamEnd;
buildChain(robot, joint, joint->after, urdfLink);
}
@@ -173,9 +199,6 @@
link->before = parent;
robot->links.push_back(link);
- if (m_verbose)
- printf("Created link '%s'\n", urdfLink->name.c_str());
-
/* copy relevant data */
switch (urdfLink->collision->geometry->type)
{
@@ -220,8 +243,6 @@
if (!found)
continue;
}
- if (m_verbose)
- printf("Connecting to link '%s'\n", urdfLink->children[i]->name.c_str());
Joint *newJoint = new Joint();
buildChain(robot, link, newJoint, urdfLink->children[i]);
link->after.push_back(newJoint);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
