Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2463] pkg/trunk
|
From: <is...@us...> - 2008-08-01 21:17:10
|
Revision: 2463
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2463&view=rev
Author: isucan
Date: 2008-08-01 21:17:18 +0000 (Fri, 01 Aug 2008)
Log Message:
-----------
better code for kinematic model
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
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
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-01 21:17:02 UTC (rev 2462)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-01 21:17:18 UTC (rev 2463)
@@ -215,10 +215,19 @@
Model *m = m_models[req.model_id];
Planner &p = m->planners[0];
- const int dim = req.goal_state.vals_size;
+ if (m->kmodel->stateDimension != req.start_state.get_vals_size())
+ {
+ fprintf(stderr, "Dimension of start state expected to be %u but was received as %u\n", m->kmodel->stateDimension, req.start_state.get_vals_size());
+ return false;
+ }
+
+ const int dim = req.goal_state.get_vals_size();
if ((int)p.si->getStateDimension() != dim)
+ {
+ fprintf(stderr, "Dimension of goal state expected to be %u but was received as %d\n", p.si->getStateDimension(), dim);
return false;
-
+ }
+
/* set the workspace volume for planning */
// 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...
@@ -310,6 +319,8 @@
}
}
+ else
+ res.path.set_states_size(0);
/* cleanup */
p.si->clearGoal();
@@ -344,30 +355,36 @@
printf("\n\nCreating new kinematic model:\n");
/* create a model for the whole robot (with the name given in the file) */
- Model *model = new Model();
planning_models::KinematicModel *kmodel = new planning_models::KinematicModel();
kmodel->setVerbose(true);
kmodel->build(*file);
+
+ /* add the model to the collision space */
unsigned int cid = m_collisionSpace->addRobotModel(kmodel);
+
+ /* set the data for the model */
+ Model *model = new Model();
model->collisionSpaceID = cid;
model->collisionSpace = m_collisionSpace;
- model->kmodel = m_collisionSpace->getModel(cid);
- m_models[file->getRobotName()] = model;
+ model->kmodel = kmodel;
createMotionPlanningInstances(model);
+ /* remember the model by the robot's name */
+ m_models[file->getRobotName()] = model;
+
+ /* create a model for each group */
std::vector<std::string> groups;
kmodel->getGroups(groups);
- /* create a model for each group */
for (unsigned int i = 0 ; i < groups.size() ; ++i)
{
Model *model = new Model();
model->collisionSpaceID = cid;
model->collisionSpace = m_collisionSpace;
- model->kmodel = m_collisionSpace->getModel(cid);
- model->groupID = model->kmodel->getGroupID(groups[i]);
+ model->kmodel = kmodel;
+ model->groupID = kmodel->getGroupID(groups[i]);
+ createMotionPlanningInstances(model);
m_models[groups[i]] = model;
- createMotionPlanningInstances(model);
}
}
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-01 21:17:02 UTC (rev 2462)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-01 21:17:18 UTC (rev 2463)
@@ -50,17 +50,19 @@
req.model_id = "pr2::base";
- req.start_state.set_vals_size(32);
+ req.start_state.set_vals_size(34);
for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
req.start_state.vals[i] = 0.0;
-
+ req.start_state.vals[0] = -0.5;
+
req.goal_state.set_vals_size(3);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
req.goal_state.vals[i] = 0.0;
- req.goal_state.vals[0] = 1.0;
+ req.goal_state.vals[0] = 0.75;
+ req.goal_state.vals[1] = 0.75;
req.goal_state.vals[2] = M_PI/2;
- req.allowed_time = 15.0;
+ req.allowed_time = 10.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;
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-08-01 21:17:02 UTC (rev 2462)
+++ pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-08-01 21:17:18 UTC (rev 2463)
@@ -84,6 +84,7 @@
axis[0] = axis[1] = axis[2] = 0.0;
anchor[0] = anchor[1] = anchor[2] = 0.0;
limit[0] = limit[1] = 0.0;
+ robotRoot = false;
type = UNKNOWN;
}
@@ -97,6 +98,9 @@
Link *before;
Link *after;
+ /** Flag for marking joints that are roots for the entire robot */
+ bool robotRoot;
+
/** The range of indices in the parameter vector that
needed to access information about the position of this
joint */
@@ -231,9 +235,6 @@
/** The model that owns this robot */
KinematicModel *owner;
- /** Group of links corresponding to this robot (if any) */
- std::string tag;
-
};
KinematicModel(void)
@@ -249,13 +250,14 @@
delete m_robots[i];
}
- virtual void build(robot_desc::URDF &model, const char *group = NULL);
+ virtual void build(robot_desc::URDF &model);
void setVerbose(bool verbose);
unsigned int getRobotCount(void) const;
Robot* getRobot(unsigned int index) const;
void getGroups(std::vector<std::string> &groups) const;
int getGroupID(const std::string &group) const;
+ Link* getLink(const std::string &link) const;
void computeTransforms(const double *params, int groupID = -1);
void printModelInfo(FILE *out = stdout) const;
@@ -286,6 +288,7 @@
std::vector<Robot*> m_robots;
std::vector<std::string> m_groups;
std::map<std::string, int> m_groupsMap;
+ std::map<std::string, Link*> m_linkMap;
bool m_verbose;
bool m_built;
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-08-01 21:17:02 UTC (rev 2462)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-08-01 21:17:18 UTC (rev 2463)
@@ -37,30 +37,23 @@
void planning_models::KinematicModel::Robot::computeTransforms(const double *params, int groupID)
{
- if (groupID >= 0)
- chain->computeTransform(params, groupID);
- else
- {
+ if (chain->robotRoot)
chain->globalTrans = owner->rootTransform;
- chain->computeTransform(params, groupID);
- }
+ chain->computeTransform(params, groupID);
}
void planning_models::KinematicModel::computeTransforms(const double *params, int groupID)
{
- if (groupID >= 0)
- for (unsigned int i = 0 ; i < groupChainStart[groupID].size() ; ++i)
- {
- groupChainStart[groupID][i]->computeTransform(params, groupID);
- params += groupStateIndexList[groupID].size();
- }
- else
- for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
- {
- m_robots[i]->chain->globalTrans = rootTransform;
- m_robots[i]->chain->computeTransform(params, groupID);
- params += m_robots[i]->stateDimension;
- }
+ unsigned int n = groupID >= 0 ? groupChainStart[groupID].size() : m_robots.size();
+
+ for (unsigned int i = 0 ; i < n ; ++i)
+ {
+ Joint *start = groupID >= 0 ? groupChainStart[groupID][i] : m_robots[i]->chain;
+ if (start->robotRoot)
+ start->globalTrans = rootTransform;
+ start->computeTransform(params, groupID);
+ params += groupID >= 0 ? groupStateIndexList[groupID].size() : m_robots[i]->stateDimension;
+ }
}
// we can optimize things here... (when we use identity transforms, for example)
@@ -127,7 +120,7 @@
m_groupsMap[m_groups[i]] = i;
}
-void planning_models::KinematicModel::build(robot_desc::URDF &model, const char *group)
+void planning_models::KinematicModel::build(robot_desc::URDF &model)
{
if (m_built)
{
@@ -142,43 +135,17 @@
groupStateIndexList.resize(m_groups.size());
groupChainStart.resize(m_groups.size());
- if (group)
+ for (unsigned int i = 0 ; i < model.getDisjointPartCount() ; ++i)
{
- robot_desc::URDF::Group *g = model.getGroup(group);
- if (g)
- {
- if (g->hasFlag("plan"))
- for (unsigned int i = 0 ; i < g->linkRoots.size() ; ++i)
- {
- robot_desc::URDF::Link *link = g->linkRoots[i];
- Robot *rb = new Robot(this);
- rb->groupStateIndexList.resize(m_groups.size());
- rb->groupChainStart.resize(m_groups.size());
- rb->tag = g->name;
- rb->chain = new Joint();
- buildChainJ(rb, NULL, rb->chain, link, model);
- m_robots.push_back(rb);
- }
- else
- fprintf(stderr, "Group '%s' is not marked for planning ('plan' flag).\n", group);
- }
- else
- fprintf(stderr, "Group '%s' not found.\n", group);
+ robot_desc::URDF::Link *link = model.getDisjointPart(i);
+ Robot *rb = new Robot(this);
+ rb->groupStateIndexList.resize(m_groups.size());
+ rb->groupChainStart.resize(m_groups.size());
+ rb->chain = new Joint();
+ buildChainJ(rb, NULL, rb->chain, link, model);
+ m_robots.push_back(rb);
}
- else
- {
- for (unsigned int i = 0 ; i < model.getDisjointPartCount() ; ++i)
- {
- robot_desc::URDF::Link *link = model.getDisjointPart(i);
- Robot *rb = new Robot(this);
- rb->groupStateIndexList.resize(m_groups.size());
- rb->groupChainStart.resize(m_groups.size());
- rb->chain = new Joint();
- buildChainJ(rb, NULL, rb->chain, link, model);
- m_robots.push_back(rb);
- }
- }
-
+
for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
{
/* copy state bounds */
@@ -202,6 +169,9 @@
groupStateIndexList[j].push_back(stateDimension + m_robots[i]->groupStateIndexList[j][k]);
stateDimension += m_robots[i]->stateDimension;
+
+ for (unsigned int j = 0 ; j < m_robots[i]->links.size() ; ++j)
+ m_linkMap[m_robots[i]->links[j]->name] = m_robots[i]->links[j];
}
}
@@ -226,11 +196,20 @@
return m_robots[index];
}
+planning_models::KinematicModel::Link* planning_models::KinematicModel::getLink(const std::string &link) const
+{
+ std::map<std::string, Link*>::const_iterator pos = m_linkMap.find(link);
+ return pos == m_linkMap.end() ? NULL : pos->second;
+}
+
void planning_models::KinematicModel::buildChainJ(Robot *robot, Link *parent, Joint* joint, robot_desc::URDF::Link* urdfLink, robot_desc::URDF &model)
{
joint->before = parent;
joint->after = new Link();
+ if (model.isRoot(urdfLink))
+ joint->robotRoot = true;
+
/* copy relevant data */
joint->limit[0] = urdfLink->joint->limit[0];
joint->limit[1] = urdfLink->joint->limit[1];
@@ -354,19 +333,6 @@
for (unsigned int i = 0 ; i < urdfLink->children.size() ; ++i)
{
- /* if building from a group of links, make sure we do not exit the group */
- if (!robot->tag.empty())
- {
- bool found = false;
- for (unsigned int k = 0 ; k < urdfLink->children[i]->groups.size() ; ++k)
- if (urdfLink->children[i]->groups[k]->name == robot->tag)
- {
- found = true;
- break;
- }
- if (!found)
- continue;
- }
Joint *newJoint = new Joint();
buildChainJ(robot, link, newJoint, urdfLink->children[i], model);
link->after.push_back(newJoint);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
