Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2396] pkg/trunk
|
From: <is...@us...> - 2008-07-31 21:47:09
|
Revision: 2396
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2396&view=rev
Author: isucan
Date: 2008-07-31 21:47:16 +0000 (Thu, 31 Jul 2008)
Log Message:
-----------
more information reporting in kinematic model building
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
pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-31 21:45:54 UTC (rev 2395)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-31 21:47:16 UTC (rev 2396)
@@ -252,6 +252,11 @@
goal->state->values[i] = req.goal_state.vals[i];
goal->threshold = 1e-6;
p.si->setGoal(goal);
+
+ printf("=======================================\n");
+ m->kmodel->printModelInfo();
+ p.si->printSettings();
+ printf("=======================================\n");
/* do the planning */
m_collisionSpace->lock();
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-31 21:45:54 UTC (rev 2395)
+++ pkg/trunk/robot_models/robot_motion_planning_models/include/planning_models/kinematic.h 2008-07-31 21:47:16 UTC (rev 2396)
@@ -39,6 +39,7 @@
#include <libTF/Pose3D.h>
#include <vector>
#include <string>
+#include <cstdio>
/** @htmlinclude ../../manifest.html
@@ -257,7 +258,8 @@
int getGroupID(const std::string &group) const;
void computeTransforms(const double *params, int groupID = -1);
-
+ void printModelInfo(FILE *out = stdout) const;
+
/** A transform that is applied to the entire model */
libTF::Pose3D rootTransform;
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-31 21:45:54 UTC (rev 2395)
+++ pkg/trunk/robot_models/robot_motion_planning_models/src/planning_models/kinematic.cpp 2008-07-31 21:47:16 UTC (rev 2396)
@@ -33,7 +33,6 @@
*********************************************************************/
#include <planning_models/kinematic.h>
-#include <cstdio>
#include <cmath>
void planning_models::KinematicModel::Robot::computeTransforms(const double *params, int groupID)
@@ -370,3 +369,41 @@
if (link->after.size() == 0)
robot->leafs.push_back(link);
}
+
+void planning_models::KinematicModel::printModelInfo(FILE *out) const
+{
+ fprintf(out, "Number of robots = %d\n", getRobotCount());
+ fprintf(out, "Complete model state dimension = %d\n", stateDimension);
+
+ fprintf(out, "State bounds: ");
+ for (unsigned int i = 0 ; i < stateDimension ; ++i)
+ fprintf(out, "[%f, %f] ", stateBounds[2 * i], stateBounds[2 * i + 1]);
+ fprintf(out, "\n");
+
+ fprintf(out, "Floating joints at: ");
+ for (unsigned int i = 0 ; i < floatingJoints.size() ; ++i)
+ fprintf(out, "%d ", floatingJoints[i]);
+ fprintf(out, "\n");
+
+ fprintf(out, "Planar joints at: ");
+ for (unsigned int i = 0 ; i < planarJoints.size() ; ++i)
+ fprintf(out, "%d ", planarJoints[i]);
+ fprintf(out, "\n");
+
+ fprintf(out, "Available groups: ");
+ std::vector<std::string> l;
+ getGroups(l);
+ for (unsigned int i = 0 ; i < l.size() ; ++i)
+ fprintf(out, "%s ", l[i].c_str());
+ fprintf(out, "\n");
+
+ for (unsigned int i = 0 ; i < l.size() ; ++i)
+ {
+ int gid = getGroupID(l[i]);
+ fprintf(out, "Group %s has %d roots\n", l[i].c_str(), groupChainStart[gid].size());
+ fprintf(out, "The state components for this group are: ");
+ for (unsigned int j = 0 ; j < groupStateIndexList[gid].size() ; ++j)
+ fprintf(out, "%d ", groupStateIndexList[gid][j]);
+ fprintf(out, "\n");
+ }
+}
Modified: pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp
===================================================================
--- pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-31 21:45:54 UTC (rev 2395)
+++ pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-31 21:47:16 UTC (rev 2396)
@@ -56,39 +56,6 @@
spaces.displaySpaces();
}
-void printModelInfo(planning_models::KinematicModel *m)
-{
- printf("Number of robots = %d\n", m->getRobotCount());
- printf("Complete model state dimension = %d\n", m->stateDimension);
-
- printf("State bounds: ");
- for (unsigned int i = 0 ; i < m->stateDimension ; ++i)
- printf("[%f, %f] ", m->stateBounds[2 * i], m->stateBounds[2 * i + 1]);
- printf("\n");
-
- printf("Floating joints at: ");
- for (unsigned int i = 0 ; i < m->floatingJoints.size() ; ++i)
- printf("%d ", m->floatingJoints[i]);
- printf("\n");
-
- printf("Available groups: ");
- std::vector<std::string> l;
- m->getGroups(l);
- for (unsigned int i = 0 ; i < l.size() ; ++i)
- printf("%s ", l[i].c_str());
- printf("\n");
-
- for (unsigned int i = 0 ; i < l.size() ; ++i)
- {
- int gid = m->getGroupID(l[i]);
- printf("Group %s has %d roots\n", l[i].c_str(), m->groupChainStart[gid].size());
- printf("The state components for this group are: ");
- for (unsigned int j = 0 ; j < m->groupStateIndexList[gid].size() ; ++j)
- printf("%d ", m->groupStateIndexList[gid][j]);
- printf("\n");
- }
-}
-
int main(int argc, char **argv)
{
// TODO: add usage message
@@ -103,7 +70,7 @@
planning_models::KinematicModel *m = new planning_models::KinematicModel();
m->build(model);
km->addRobotModel(m);
- printModelInfo(m);
+ m->printModelInfo();
double *param = new double[m->stateDimension];
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
