[Personalrobots-commit] SF.net SVN: personalrobots:[17063] pkg/trunk/motion_planning/ planning_models

[<is...@us...>]

Revision: 17063
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=17063&view=rev
Author:   isucan
Date:     2009-06-13 04:44:58 +0000 (Sat, 13 Jun 2009)

Log Message:
-----------
one more utility function

Modified Paths:
--------------
    pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
    pkg/trunk/motion_planning/planning_models/src/kinematic.cpp

Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h	2009-06-13 04:44:13 UTC (rev 17062)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h	2009-06-13 04:44:58 UTC (rev 17063)
@@ -585,7 +585,7 @@
 	unsigned int getRobotCount(void) const;
 	Robot*       getRobot(unsigned int index) const;
 
-	/** Get the array of groups, indexed by their group ID */
+	/** Get the array of planning groups, indexed by their group ID */
 	void         getGroups(std::vector<std::string> &groups) const;
 	
 	/** Get the number of groups */
@@ -614,6 +614,9 @@
 	    same as in the group state */
 	void         getJointsInGroup(std::vector<std::string> &names, const std::string &name) const;
 
+	/** Return the index of a joint in the complete state vector */
+	int getJointIndex(const std::string &name) const;
+
 	/** Get the index for the parameter of a joint in a given group */
 	int getJointIndexInGroup(const std::string &name, const std::string &group) const;
 

Modified: pkg/trunk/motion_planning/planning_models/src/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/kinematic.cpp	2009-06-13 04:44:13 UTC (rev 17062)
+++ pkg/trunk/motion_planning/planning_models/src/kinematic.cpp	2009-06-13 04:44:58 UTC (rev 17063)
@@ -826,6 +826,15 @@
     return m_params;
 }
 
+int planning_models::KinematicModel::getJointIndex(const std::string &name) const
+{
+    std::map<std::string, unsigned int>::const_iterator it = m_mi.parameterIndex.find(name);
+    if (it != m_mi.parameterIndex.end())
+	return it->second;
+    m_msg.error("Joint " + name + " not found");
+    return -1;
+}
+
 int planning_models::KinematicModel::getJointIndexInGroup(const std::string &name, const std::string &group) const
 {
     return getJointIndexInGroup(name, getGroupID(group));


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.