Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[19132] pkg/trunk/motion_planning/ planning_models
|
From: <is...@us...> - 2009-07-17 20:23:12
|
Revision: 19132
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=19132&view=rev
Author: isucan
Date: 2009-07-17 20:23:10 +0000 (Fri, 17 Jul 2009)
Log Message:
-----------
cloning for kinematic models works
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-07-17 20:15:00 UTC (rev 19131)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp 2009-07-17 20:23:10 UTC (rev 19132)
@@ -547,17 +547,23 @@
// for each group, keep track of the indices from the robot state that correspond to it
for (unsigned int i = 0 ; i < joint->inGroup.size() ; ++i)
if (joint->inGroup[i])
+ {
for (unsigned int j = 0 ; j < joint->usedParams ; ++j)
robot->groupStateIndexList[i].push_back(j + robot->stateDimension);
-
- // check if the current link has parents in this group
- // if it does not, it is a root link, so we keep track of it
- for (unsigned int k = 0 ; k < urdfLink->groups.size() ; ++k)
- if (urdfLink->groups[k]->isRoot(urdfLink))
- {
- std::string gname = urdfLink->groups[k]->name;
- if (m_groupsMap.find(gname) != m_groupsMap.end())
- robot->groupChainStart[m_groupsMap[gname]].push_back(joint);
+
+ // if we have a parent link, check if that link is in the same group as we are
+ bool found = false;
+ if (parent)
+ {
+ for (unsigned int j = 0 ; j < m_groupContent[m_groups[i]].size() ; ++j)
+ if (parent->name == m_groupContent[m_groups[i]][j])
+ {
+ found = true;
+ break;
+ }
+ }
+ if (!found)
+ robot->groupChainStart[i].push_back(joint);
}
if (m_verbose && joint->usedParams > 0)
@@ -802,6 +808,7 @@
if (newJoint)
{
+ newJoint->name = joint->name;
newJoint->usedParams = joint->usedParams;
newJoint->inGroup = joint->inGroup;
newJoint->varTrans = joint->varTrans;
@@ -855,8 +862,7 @@
km->m_built = m_built;
km->m_verbose = m_verbose;
km->m_name = m_name;
- km->m_mi = m_mi;
-
+
for (unsigned int i = 0 ; i < m_robots.size() ; ++i)
{
Robot *r = new Robot(km);
Modified: pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp 2009-07-17 20:15:00 UTC (rev 19131)
+++ pkg/trunk/motion_planning/planning_models/test/test_kinematic.cpp 2009-07-17 20:23:10 UTC (rev 19132)
@@ -126,61 +126,8 @@
" <elem key=\"my_key\" value=\"my_key_val\"/>"
" </map>"
"</link>"
- "<group name=\"base\" flags=\"planning\">"
- "base_link"
- "</group>"
"</robot>";
- static const std::string MODEL1_PARSED =
- "\n"
- "List of root links in robot 'one_robot' (1) :\n"
- " Link [base_link]:\n"
- " - parent link: world\n"
- " - rpy: (0, 0, 1)\n"
- " - xyz: (0.1, 0, 0)\n"
- " Joint [base_link_joint]:\n"
- " - type: 4\n"
- " - axis: (0, 0, 0)\n"
- " - anchor: (0, 0, 0)\n"
- " - limit: (0, 0)\n"
- " - effortLimit: 0\n"
- " - velocityLimit: 0\n"
- " - calibration: \n"
- " Collision [my_collision]:\n"
- " - verbose: No\n"
- " - rpy: (0, 0, -1)\n"
- " - xyz: (-0.1, 0, 0)\n"
- " Geometry [my_collision_geom]:\n"
- " - type: 2\n"
- " - size: (1, 2, 1)\n"
- " Inertial [base_link_inertial]:\n"
- " - mass: 2.81\n"
- " - com: (0, 0.099, 0)\n"
- " - inertia: (0.1, -0.2, 0.5, -0.09, 1, 0.101)\n"
- " Visual [base_link_visual]:\n"
- " - rpy: (0, 0, 0)\n"
- " - xyz: (0, 0, 0)\n"
- " Geometry [base_link_visual_geom]:\n"
- " - type: 2\n"
- " - size: (1, 2, 1)\n"
- " - groups: base ( planning ) \n"
- " - children links: \n"
- " Data flagged as '':\n"
- " []\n"
- " my_key = my_key_val\n"
- "\n"
- "Frames:\n"
- "\n"
- "Groups:\n"
- " Group [base]:\n"
- " - links: base_link \n"
- " - frames: \n"
- " - flags: planning \n"
- "\n"
- "Chains:\n"
- "\n"
- "Data types:\n";
-
static const std::string MODEL1_INFO =
"Number of robots = 1\n"
"Complete model state dimension = 3\n"
@@ -216,10 +163,6 @@
EXPECT_EQ((unsigned int)1, model->getModelInfo().planarJoints.size());
EXPECT_EQ(0, model->getModelInfo().planarJoints[0]);
-
- std::stringstream ssp;
- file->print(ssp);
- EXPECT_EQ(MODEL1_PARSED, ssp.str());
std::stringstream ssi;
model->printModelInfo(ssi);
@@ -339,12 +282,6 @@
" </geometry>"
" </visual>"
"</link>"
- "<group name=\"base\" flags=\"planning\">"
- "base_link "
- "link_a "
- "link_b "
- "link_c "
- "</group>"
"</robot>";
static const std::string MODEL2_INFO =
@@ -671,37 +608,7 @@
" <box size=\"1 2 1\" />"
" </geometry>"
" </visual>"
- "</link>"
- "<group name=\"r1\" flags=\"planning\">"
- "base_link1 "
- "link_a "
- "link_b "
- "link_c "
- "</group>"
- "<group name=\"r2\" flags=\"planning\">"
- "base_link2 "
- "link_d "
- "link_e "
- "link_f "
- "</group>"
- "<group name=\"r1r2\" flags=\"planning\">"
- "base_link1 "
- "link_a "
- "link_b "
- "link_c "
- "base_link2 "
- "link_d "
- "link_e "
- "link_f "
- "</group>"
- "<group name=\"parts\" flags=\"planning\">"
- "base_link1 "
- "link_a "
- "link_b "
- "link_e "
- "link_f "
- "base_link3 "
- "</group>"
+ "</link>"
"</robot>";
static const std::string MODEL3_INFO =
@@ -774,17 +681,20 @@
model->reduceToRobotFrame();
EXPECT_EQ((unsigned int)13, model->getModelInfo().stateDimension);
- // planning_models::KinematicModel *clone = model->clone();
- // delete model;
- // model = clone;
+ planning_models::KinematicModel *clone = model->clone();
+ delete model;
+ model = clone;
std::stringstream ss;
model->printModelInfo(ss);
- printf("%s\n", ss.str().c_str());
double param[8] = { -1, -1, 0, 1.57, 0.0, 5, 5, 0 };
model->computeTransformsGroup(param, model->getGroupID("parts"));
+ clone = model->clone();
+ delete model;
+ model = clone;
+
EXPECT_TRUE(sameStringIgnoringWS(MODEL3_INFO, ss.str()));
EXPECT_NEAR(-1.0, model->getLink("base_link1")->globalTrans.getOrigin().x(), 1e-5);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
