Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[2342] pkg/trunk
|
From: <is...@us...> - 2008-07-30 22:13:09
|
Revision: 2342
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=2342&view=rev
Author: isucan
Date: 2008-07-30 22:13:15 +0000 (Wed, 30 Jul 2008)
Log Message:
-----------
use new group information in kinematic planning
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/util/collision_space/include/collision_space/environment.h
pkg/trunk/util/collision_space/src/collision_space/environment.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-30 21:55:02 UTC (rev 2341)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-07-30 22:13:15 UTC (rev 2342)
@@ -299,44 +299,49 @@
void addRobotDescriptionFromFile(const char *filename)
{
- robot_desc::URDF *file = new robot_desc::URDF(filename);
- addRobotDescription(file);
+ robot_desc::URDF *file = new robot_desc::URDF();
+ if (file->loadFile(filename))
+ addRobotDescription(file);
+ else
+ delete file;
}
void addRobotDescriptionFromData(const char *data)
{
robot_desc::URDF *file = new robot_desc::URDF();
- file->loadString(data);
- addRobotDescription(file);
+ if (file->loadString(data))
+ addRobotDescription(file);
+ else
+ delete file;
}
void addRobotDescription(robot_desc::URDF *file)
{
m_robotDescriptions.push_back(file);
- std::vector<std::string> groups;
- file->getGroupNames(groups);
- std::string name = file->getRobotName();
-
/* create a model for the whole robot (with the name given in the file) */
Model *model = new Model();
- unsigned int cid = m_collisionSpace->addRobotModel(*file);
+ planning_models::KinematicModel *kmodel = new planning_models::KinematicModel();
+ kmodel->build(*file);
+ unsigned int cid = m_collisionSpace->addRobotModel(kmodel);
model->collisionSpaceID = cid;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_collisionSpace->getModel(cid);
m_models[name] = model;
createMotionPlanningInstances(model);
+ 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();
- std::string gname = name + "::" + groups[i];
model->collisionSpaceID = cid;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_collisionSpace->getModel(cid);
- model->groupID = model->kmodel->getGroupID(gname);
- m_models[gname] = model;
+ model->groupID = model->kmodel->getGroupID(groups[i]);
+ 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-07-30 21:55:02 UTC (rev 2341)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-07-30 22:13:15 UTC (rev 2342)
@@ -48,7 +48,7 @@
robot_srvs::KinematicMotionPlan::request req;
robot_srvs::KinematicMotionPlan::response res;
- req.model_id = "pr2::leftArm";
+ req.model_id = "pr2::base+rightArm";
req.start_state.set_vals_size(32);
for (unsigned int i = 0 ; i < req.start_state.vals_size ; ++i)
@@ -57,11 +57,11 @@
for (unsigned int i = 18 ; i < 25 ; ++i)
req.start_state.vals[i] = 0.1;
- req.goal_state.set_vals_size(7);
+ req.goal_state.set_vals_size(11);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
req.goal_state.vals[i] = -0.4;
- req.allowed_time = 2.0;
+ req.allowed_time = 15.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/util/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-30 21:55:02 UTC (rev 2341)
+++ pkg/trunk/util/collision_space/include/collision_space/environment.h 2008-07-30 22:13:15 UTC (rev 2342)
@@ -71,8 +71,6 @@
virtual void addPointCloud(unsigned int n, const double* points, double radius = 0.01) = 0;
/** Add a robot model */
- virtual unsigned int addRobotModel(robot_desc::URDF &pmodel, const char *group = NULL);
- /** Add a robot model */
virtual unsigned int addRobotModel(planning_models::KinematicModel *model);
/** Update the positions of the geometry used in collision detection */
Modified: pkg/trunk/util/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/util/collision_space/src/collision_space/environment.cpp 2008-07-30 21:55:02 UTC (rev 2341)
+++ pkg/trunk/util/collision_space/src/collision_space/environment.cpp 2008-07-30 22:13:15 UTC (rev 2342)
@@ -34,13 +34,6 @@
#include <collision_space/environment.h>
-unsigned int collision_space::EnvironmentModel::addRobotModel(robot_desc::URDF &pmodel, const char *group)
-{
- planning_models::KinematicModel *m = new planning_models::KinematicModel();
- m->build(pmodel, group);
- return addRobotModel(m);
-}
-
unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model)
{
unsigned int pos = m_models.size();
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-30 21:55:02 UTC (rev 2341)
+++ pkg/trunk/util/test_collision_space/src/test_kinematic_ode.cpp 2008-07-30 22:13:15 UTC (rev 2342)
@@ -100,8 +100,9 @@
model.loadFile("/u/isucan/ros/ros-pkg/robot_descriptions/wg_robot_description/pr2/pr2.xml");
EnvironmentModel *km = new EnvironmentModelODE();
- km->addRobotModel(model);
- planning_models::KinematicModel *m = km->getModel(0);
+ planning_models::KinematicModel *m = new planning_models::KinematicModel();
+ m->build(model);
+ km->addRobotModel(m);
printModelInfo(m);
double *param = new double[m->stateDimension];
@@ -113,8 +114,8 @@
for (unsigned int i = 0 ; i < m->stateDimension ; ++i)
- param[i] = -0.3;
- m->computeTransforms(param, m->getGroupID("pr2::leftArm"));
+ param[i] = +0.3;
+ m->computeTransforms(param, m->getGroupID("pr2::base+rightArm"));
km->updateRobotModel(0);
EnvironmentModelODE* okm = dynamic_cast<EnvironmentModelODE*>(km);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
