Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[23340] pkg/trunk/motion_planning/ planning_models
|
From: <is...@us...> - 2009-08-30 00:28:06
|
Revision: 23340
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=23340&view=rev
Author: isucan
Date: 2009-08-30 00:27:56 +0000 (Sun, 30 Aug 2009)
Log Message:
-----------
a bit more stuff... still far from complete
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_models/CMakeLists.txt
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h
pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp
Added Paths:
-----------
pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp
Modified: pkg/trunk/motion_planning/planning_models/CMakeLists.txt
===================================================================
--- pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-08-29 20:57:27 UTC (rev 23339)
+++ pkg/trunk/motion_planning/planning_models/CMakeLists.txt 2009-08-30 00:27:56 UTC (rev 23340)
@@ -18,3 +18,7 @@
rospack_add_gtest(test_kinematic test/test_kinematic.cpp)
target_link_libraries(test_kinematic planning_models)
rospack_link_boost(test_kinematic thread)
+
+#rospack_add_gtest(test_kinematic2 test/test_kinematic2.cpp)
+#target_link_libraries(test_kinematic2 planning_models2)
+#rospack_link_boost(test_kinematic2 thread)
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h 2009-08-29 20:57:27 UTC (rev 23339)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic_model.h 2009-08-30 00:27:56 UTC (rev 23340)
@@ -285,6 +285,9 @@
/** \brief The dimension of the group */
unsigned int dimension;
+ /** \brief The bounds for the state corresponding to the group */
+ std::vector<double> stateBounds;
+
/** \brief An array containing the index in the global state for each dimension of the state of the group */
std::vector<unsigned int> stateIndex;
@@ -372,6 +375,11 @@
/** \brief Get the dimension of the entire model */
unsigned int getDimension(void) const;
+ /** \brief Get the state bounds constructed for this
+ model. Component i of the state space has bounds (min,
+ max) at index positions (2*i, 2*i+1)*/
+ const std::vector<double> &getStateBounds(void) const;
+
/** \brief Provide interface to a lock. Use carefully! */
void lock(void);
@@ -407,6 +415,15 @@
/** \brief The root joint */
Joint *root_;
+ /** \brief List of floating joints, maintained for convenience */
+ std::vector<std::string> floatingJoints_;
+
+ /** \brief List of planar joints, maintained for convenience */
+ std::vector<std::string> planarJoints_;
+
+ /** \brief The bounds in the form (min, max) for every component of the state */
+ std::vector<double> stateBounds_;
+
/** \brief The dimension of the model */
unsigned int dimension_;
@@ -417,7 +434,7 @@
void buildGroups(const std::map< std::string, std::vector<std::string> > &groups);
Joint* buildRecursive(Link *parent, const urdf::Link *link);
- Joint* constructJoint(const urdf::Joint *urdfJoint);
+ Joint* constructJoint(const urdf::Joint *urdfJoint, std::vector<double> &bounds);
Link* constructLink(const urdf::Link *urdfLink);
shapes::Shape* constructShape(const urdf::Geometry *geom);
Modified: pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp 2009-08-29 20:57:27 UTC (rev 23339)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic_model.cpp 2009-08-30 00:27:56 UTC (rev 23340)
@@ -45,6 +45,8 @@
planning_models::KinematicModel::KinematicModel(const urdf::Model &model, const std::map< std::string, std::vector<std::string> > &groups)
{
dimension_ = 0;
+ modelName_ = model.getName();
+
if (model.getRoot())
{
root_ = buildRecursive(NULL, model.getRoot().get());
@@ -75,6 +77,11 @@
return dimension_;
}
+const std::vector<double> &planning_models::KinematicModel::getStateBounds(void) const
+{
+ return stateBounds_;
+}
+
const btTransform& planning_models::KinematicModel::getRootTransform(void) const
{
return rootTransform_;
@@ -124,7 +131,9 @@
planning_models::KinematicModel::Joint* planning_models::KinematicModel::buildRecursive(Link *parent, const urdf::Link *link)
{
- Joint *joint = constructJoint(link->parent_joint.get());
+ ROS_INFO("%s", link->name.c_str());
+
+ Joint *joint = constructJoint(link->parent_joint.get(), stateBounds_);
joint->stateIndex = dimension_;
jointMap_[joint->name] = joint;
jointList_.push_back(joint);
@@ -135,13 +144,19 @@
linkMap_[joint->after->name] = joint->after;
joint->after->before = joint;
+ if (dynamic_cast<FloatingJoint*>(joint))
+ floatingJoints_.push_back(joint->name);
+ if (dynamic_cast<PlanarJoint*>(joint))
+ planarJoints_.push_back(joint->name);
+
for (unsigned int i = 0 ; link->child_links.size() ; ++i)
joint->after->after.push_back(buildRecursive(joint->after, link->child_links[i].get()));
return joint;
}
-planning_models::KinematicModel::Joint* planning_models::KinematicModel::constructJoint(const urdf::Joint *urdfJoint)
+planning_models::KinematicModel::Joint* planning_models::KinematicModel::constructJoint(const urdf::Joint *urdfJoint,
+ std::vector<double> &bounds)
{
planning_models::KinematicModel::Joint *result = NULL;
@@ -157,6 +172,8 @@
j->lowLimit = urdfJoint->safety->soft_lower_limit;
j->continuous = false;
j->axis.setValue(urdfJoint->axis.x, urdfJoint->axis.y, urdfJoint->axis.z);
+ bounds.push_back(j->lowLimit);
+ bounds.push_back(j->hiLimit);
result = j;
}
break;
@@ -167,6 +184,8 @@
j->lowLimit = -M_PI;
j->continuous = true;
j->axis.setValue(urdfJoint->axis.x, urdfJoint->axis.y, urdfJoint->axis.z);
+ bounds.push_back(j->lowLimit);
+ bounds.push_back(j->hiLimit);
result = j;
}
break;
@@ -177,14 +196,20 @@
j->hiLimit = urdfJoint->safety->soft_upper_limit;
j->lowLimit = urdfJoint->safety->soft_lower_limit;
j->axis.setValue(urdfJoint->axis.x, urdfJoint->axis.y, urdfJoint->axis.z);
+ bounds.push_back(j->lowLimit);
+ bounds.push_back(j->hiLimit);
result = j;
}
break;
case urdf::Joint::FLOATING:
result = new FloatingJoint(this);
+ bounds.insert(bounds.end(), 14, 0.0);
break;
case urdf::Joint::PLANAR:
result = new PlanarJoint(this);
+ bounds.insert(bounds.end(), 4, 0.0);
+ bounds.push_back(-M_PI);
+ bounds.push_back(M_PI);
break;
case urdf::Joint::FIXED:
result = new FixedJoint(this);
@@ -421,12 +446,66 @@
void planning_models::KinematicModel::printModelInfo(std::ostream &out) const
{
+ out << "Complete model state dimension = " << dimension_ << std::endl;
+
+ std::ios_base::fmtflags old_flags = out.flags();
+ out.setf(std::ios::fixed, std::ios::floatfield);
+ std::streamsize old_prec = out.precision();
+ out.precision(5);
+ out << "State bounds: ";
+ for (unsigned int i = 0 ; i < dimension_ ; ++i)
+ out << "[" << stateBounds_[2 * i] << ", " << stateBounds_[2 * i + 1] << "] ";
+ out << std::endl;
+ out.precision(old_prec);
+ out.flags(old_flags);
+
+ out << "Floating joints : ";
+ for (unsigned int i = 0 ; i < floatingJoints_.size() ; ++i)
+ out << floatingJoints_[i] << " ";
+ out << std::endl;
+
+ out << "Planar joints : ";
+ for (unsigned int i = 0 ; i < planarJoints_.size() ; ++i)
+ out << planarJoints_[i] << " ";
+ out << std::endl;
+
+ out << "Available groups: ";
+ std::vector<std::string> l;
+ getGroupNames(l);
+ for (unsigned int i = 0 ; i < l.size() ; ++i)
+ out << l[i] << " ";
+ out << std::endl;
+
+ for (unsigned int i = 0 ; i < l.size() ; ++i)
+ {
+ const JointGroup *g = getGroup(l[i]);
+ out << "Group " << l[i] << " has " << g->jointRoots.size() << " roots: ";
+ for (unsigned int j = 0 ; j < g->jointRoots.size() ; ++j)
+ out << g->jointRoots[j]->name << " ";
+ out << std::endl;
+ out << "The state components for this group are: ";
+ for (unsigned int j = 0 ; j < g->stateIndex.size() ; ++j)
+ out << g->stateIndex[j] << " ";
+ out << std::endl;
+ }
}
void planning_models::KinematicModel::printLinkPoses(std::ostream &out) const
{
+ out << "Link poses:" << std::endl;
+ std::vector<const Link*> links;
+ getLinks(links);
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ {
+ out << links[i]->name << std::endl;
+ const btVector3 &v = links[i]->globalTrans.getOrigin();
+ out << " origin: " << v.x() << ", " << v.y() << ", " << v.z() << std::endl;
+ const btQuaternion &q = links[i]->globalTrans.getRotation();
+ out << " quaternion: " << q.x() << ", " << q.y() << ", " << q.z() << ", " << q.w() << std::endl;
+ out << std::endl;
+ }
}
-
+
/* ------------------------ Joint ------------------------ */
planning_models::KinematicModel::Joint::Joint(KinematicModel *model) : owner(model), usedParams(0), stateIndex(0), before(NULL), after(NULL)
@@ -531,6 +610,7 @@
std::vector<const Joint*> allJoints;
owner->getJoints(allJoints);
+ std::vector<double> allBounds = owner->getStateBounds();
for (unsigned int i = 0 ; i < joints.size() ; ++i)
{
@@ -545,7 +625,12 @@
if (allJoints[j]->name == joints[i]->name)
{
for (unsigned int k = 0 ; k < joints[i]->usedParams ; ++k)
- stateIndex.push_back(globalStateIndex + k);
+ {
+ unsigned int si = globalStateIndex + k;
+ stateIndex.push_back(si);
+ stateBounds.push_back(allBounds[2 * si]);
+ stateBounds.push_back(allBounds[2 * si + 1]);
+ }
found = true;
break;
}
Added: pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp (rev 0)
+++ pkg/trunk/motion_planning/planning_models/test/test_kinematic2.cpp 2009-08-30 00:27:56 UTC (rev 23340)
@@ -0,0 +1,106 @@
+/*********************************************************************
+* 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.
+*********************************************************************/
+
+/** \author Ioan Sucan */
+
+#include <planning_models/kinematic_model.h>
+#include <gtest/gtest.h>
+#include <sstream>
+#include <ctype.h>
+
+static bool sameStringIgnoringWS(const std::string &s1, const std::string &s2)
+{
+ unsigned int i1 = 0;
+ unsigned int i2 = 0;
+ while (i1 < s1.size() && isspace(s1[i1])) i1++;
+ while (i2 < s2.size() && isspace(s2[i2])) i2++;
+ while (i1 < s1.size() && i2 < s2.size())
+ {
+ if (i1 < s1.size() && i2 < s2.size())
+ {
+ if (s1[i1] != s2[i2])
+ return false;
+ i1++;
+ i2++;
+ }
+ while (i1 < s1.size() && isspace(s1[i1])) i1++;
+ while (i2 < s2.size() && isspace(s2[i2])) i2++;
+ }
+ return i1 == s1.size() && i2 == s2.size();
+}
+
+TEST(Loading, EmptyRobot)
+{
+ static const std::string MODEL0 =
+ "<?xml version=\"1.0\" ?>"
+ "<robot name=\"myrobot\">"
+ " <joint name=\"base_joint\" type=\"planar\">"
+ " <origin rpy=\"0 0 0\" xyz=\"0 0 0.051\"/>"
+ " <parent link=\"world\"/>"
+ " <child link=\"base_link\"/>"
+ "</joint>"
+ "<link name=\"base_link\">"
+ "</link>"
+ "</robot>";
+
+ urdf::Model urdfModel;
+ urdfModel.initString(MODEL0);
+
+ std::map < std::string, std::vector<std::string> > groups;
+ planning_models::KinematicModel *model = new planning_models::KinematicModel(urdfModel, groups);
+
+ EXPECT_EQ(std::string("myrobot"), model->getName());
+ EXPECT_EQ((unsigned int)0, model->getDimension());
+
+ std::vector<const planning_models::KinematicModel::Link*> links;
+ model->getLinks(links);
+ EXPECT_EQ((unsigned int)0, links.size());
+
+ std::vector<const planning_models::KinematicModel::Joint*> joints;
+ model->getJoints(joints);
+ EXPECT_EQ((unsigned int)0, joints.size());
+
+ std::vector<std::string> pgroups;
+ model->getGroupNames(pgroups);
+ EXPECT_EQ((unsigned int)0, pgroups.size());
+
+ delete model;
+}
+
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
