Menu
▾
▴
personalrobots-commit
|
From: <is...@us...> - 2008-08-22 17:35:53
|
Revision: 3480
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3480&view=rev
Author: isucan
Date: 2008-08-22 17:36:01 +0000 (Fri, 22 Aug 2008)
Log Message:
-----------
computed some good parameters for rrt and lazy rrt, automatically loading them from the parameter server
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h
pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-08-22 17:36:01 UTC (rev 3480)
@@ -8,5 +8,6 @@
<depend package="planning_node_util" />
<depend package="ompl" />
<depend package="profiling_utils" />
+ <depend package="string_utils" />
<depend package="gtest" />
</package>
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-22 17:36:01 UTC (rev 3480)
@@ -98,6 +98,7 @@
#include "SpaceInformationXMLModel.h"
#include <profiling_utils/profiler.h>
+#include <string_utils/string_utils.h>
#include <iostream>
#include <sstream>
@@ -115,11 +116,6 @@
robot_model)
{
advertise_service("plan_kinematic_path_state", &KinematicPlanning::plan);
-
-
- double sphere[3] = {0.7, 0.5, 0.7 };
- m_collisionSpace->addPointCloud(1, sphere, 0.2);
-
}
~KinematicPlanning(void)
@@ -285,10 +281,11 @@
model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
+ model->groupName = m_kmodel->name;
createMotionPlanningInstances(model);
/* remember the model by the robot's name */
- m_models[m_urdf->getRobotName()] = model;
+ m_models[model->groupName] = model;
printf("=======================================\n");
m_kmodel->printModelInfo();
@@ -305,8 +302,9 @@
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
model->groupID = m_kmodel->getGroupID(groups[i]);
+ model->groupName = groups[i];
createMotionPlanningInstances(model);
- m_models[groups[i]] = model;
+ m_models[model->groupName] = model;
}
}
@@ -352,7 +350,8 @@
collision_space::EnvironmentModel *collisionSpace;
unsigned int collisionSpaceID;
planning_models::KinematicModel *kmodel;
- int groupID;
+ std::string groupName;
+ int groupID;
};
class StateValidityPredicate : public ompl::SpaceInformation::StateValidityChecker
@@ -377,7 +376,13 @@
Model *m_model;
};
-
+ robot_desc::URDF::Group* getURDFGroup(const std::string &gname)
+ {
+ std::string urdfGroup = gname;
+ urdfGroup.erase(0, urdfGroup.find_last_of(":") + 1);
+ return m_urdf->getGroup(urdfGroup);
+ }
+
void createMotionPlanningInstances(Model* model)
{
addRRTInstance(model);
@@ -386,29 +391,81 @@
void addRRTInstance(Model *model)
{
Planner p;
+ std::cout << "Adding RRT instance for motion planning: " << model->groupName << std::endl;
+
p.si = new SpaceInformationXMLModel(model->kmodel, model->groupID);
p.svc = new StateValidityPredicate(model);
p.si->setStateValidityChecker(p.svc);
+
p.smoother = new ompl::PathSmootherKinematic(p.si);
- p.smoother->setMaxSteps(20);
- p.mp = new ompl::RRT(p.si);
+ p.smoother->setMaxSteps(20);
+
+ ompl::RRT_t rrt = new ompl::RRT(p.si);
+ p.mp = rrt;
+
+ robot_desc::URDF::Group *group = getURDFGroup(model->groupName);
+ if (group)
+ {
+ const robot_desc::URDF::Map &data = group->data;
+ std::map<std::string, std::string> info = data.getMapTagValues("plan", "RRT");
+
+ if (info.find("range") != info.end())
+ {
+ double range = string_utils::fromString<double>(info["range"]);
+ rrt->setRange(range);
+ std::cout << "Range is set to " << range << std::endl;
+ }
+
+ if (info.find("goal_bias") != info.end())
+ {
+ double bias = string_utils::fromString<double>(info["goal_bias"]);
+ rrt->setGoalBias(bias);
+ std::cout << "Goal bias is set to " << bias << std::endl;
+ }
+ }
+
p.si->setup();
model->planners.push_back(p);
- std::cout << "Added RRT instance for motion planning" << std::endl;
}
void addLazyRRTInstance(Model *model)
{
Planner p;
+ std::cout << "Added LazyRRT instance for motion planning: " << model->groupName << std::endl;
+
p.si = new SpaceInformationXMLModel(model->kmodel, model->groupID);
p.svc = new StateValidityPredicate(model);
p.si->setStateValidityChecker(p.svc);
+
p.smoother = new ompl::PathSmootherKinematic(p.si);
- p.smoother->setMaxSteps(20);
- p.mp = new ompl::LazyRRT(p.si);
+ p.smoother->setMaxSteps(20);
+
+ ompl::LazyRRT_t lrrt = new ompl::LazyRRT(p.si);
+ p.mp = lrrt;
+
+ robot_desc::URDF::Group *group = getURDFGroup(model->groupName);
+ if (group)
+ {
+ const robot_desc::URDF::Map &data = group->data;
+ std::map<std::string, std::string> info = data.getMapTagValues("plan", "LazyRRT");
+
+ if (info.find("range") != info.end())
+ {
+ double range = string_utils::fromString<double>(info["range"]);
+ lrrt->setRange(range);
+ std::cout << "Range is set to " << range << std::endl;
+ }
+
+ if (info.find("goal_bias") != info.end())
+ {
+ double bias = string_utils::fromString<double>(info["goal_bias"]);
+ lrrt->setGoalBias(bias);
+ std::cout << "Goal bias is set to " << bias << std::endl;
+ }
+ }
+
p.si->setup();
model->planners.push_back(p);
- std::cout << "Added LazyRRT instance for motion planning" << std::endl;
}
std::map<std::string, Model*> m_models;
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h 2008-08-22 17:36:01 UTC (rev 3480)
@@ -56,7 +56,7 @@
m_nn.setDataParameter(reinterpret_cast<void*>(dynamic_cast<SpaceInformationKinematic_t>(m_si)));
random_utils::random_init(&m_rngState);
m_goalBias = 0.05;
- m_rho = 0.5;
+ m_rho = 0.1;
}
virtual ~LazyRRT(void)
Modified: pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h
===================================================================
--- pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/ompl/code/ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h 2008-08-22 17:36:01 UTC (rev 3480)
@@ -55,7 +55,7 @@
m_nn.setDataParameter(reinterpret_cast<void*>(dynamic_cast<SpaceInformationKinematic_t>(m_si)));
random_utils::random_init(&m_rngState);
m_goalBias = 0.05;
- m_rho = 0.5;
+ m_rho = 0.1;
}
virtual ~RRT(void)
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-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-22 17:36:01 UTC (rev 3480)
@@ -104,7 +104,7 @@
req.threshold = 0.01;
req.distance_metric = "L2Square";
req.interpolate = 1;
- req.times = 5;
+ req.times = 100;
initialState(req.start_state);
@@ -130,7 +130,7 @@
req.threshold = 0.01;
req.distance_metric = "L2Square";
req.interpolate = 1;
- req.times = 1;
+ req.times = 100;
initialState(req.start_state);
Modified: pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h
===================================================================
--- pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/planning_models/include/planning_models/kinematic.h 2008-08-22 17:36:01 UTC (rev 3480)
@@ -344,9 +344,6 @@
void computeTransforms(const double *params, int groupID = -1);
- /** The name of the robot */
- std::string name;
-
/** The model that owns this robot */
KinematicModel *owner;
@@ -421,6 +418,9 @@
void printModelInfo(std::ostream &out = std::cout) const;
void printLinkPoses(std::ostream &out = std::cout) const;
+ /** The name of the model */
+ std::string name;
+
/** A transform that is applied to the entire model */
libTF::Pose3D rootTransform;
Modified: pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp 2008-08-22 17:36:01 UTC (rev 3480)
@@ -264,7 +264,8 @@
m_built = true;
m_ignoreSensors = ignoreSensors;
-
+ name = model.getRobotName();
+
/* construct a map for the available groups */
constructGroupList(model);
groupStateIndexList.resize(m_groups.size());
@@ -276,7 +277,6 @@
if (link->canSense() && m_ignoreSensors)
continue;
Robot *rb = new Robot(this);
- rb->name = model.getRobotName();
rb->groupStateIndexList.resize(m_groups.size());
rb->groupChainStart.resize(m_groups.size());
rb->chain = createJoint(link);
@@ -373,7 +373,7 @@
for (unsigned int k = 0 ; k < urdfLink->groups.size() ; ++k)
if (urdfLink->groups[k]->isRoot(urdfLink))
{
- std::string gname = robot->name + "::" + urdfLink->groups[k]->name;
+ std::string gname = name + "::" + urdfLink->groups[k]->name;
if (m_groupsMap.find(gname) != m_groupsMap.end())
robot->groupChainStart[m_groupsMap[gname]].push_back(joint);
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-22 17:18:24 UTC (rev 3479)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-22 17:36:01 UTC (rev 3480)
@@ -38,6 +38,14 @@
forearm_roll_left
wrist_flex_left
gripper_roll_left
+
+ <map name="RRT" flag="plan">
+ <elem key="range" value="0.75" />
+ </map>
+
+ <map name="LazyRRT" flag="plan">
+ <elem key="range" value="0.75" />
+ </map>
</group>
<group name="rightArm" flags="plan kinematic">
@@ -50,9 +58,13 @@
wrist_flex_right
gripper_roll_right
- <map name="rrt" flag="plan">
- <elem key="rho" value="0.5" />
+ <map name="RRT" flag="plan">
+ <elem key="range" value="0.75" />
</map>
+
+ <map name="LazyRRT" flag="plan">
+ <elem key="range" value="0.75" />
+ </map>
</group>
<group name="base+leftArm" flags="plan">
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-22 17:50:56
|
Revision: 3484
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3484&view=rev
Author: isucan
Date: 2008-08-22 17:51:04 +0000 (Fri, 22 Aug 2008)
Log Message:
-----------
small incremental updates
Modified Paths:
--------------
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp
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-08-22 17:47:41 UTC (rev 3483)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-22 17:51:04 UTC (rev 3484)
@@ -79,13 +79,17 @@
req.threshold = 0.01;
req.distance_metric = "L2Square";
req.interpolate = 1;
- req.times = 10;
+ req.times = 1;
initialState(req.start_state);
req.goal_state.set_vals_size(3);
for (unsigned int i = 0 ; i < req.goal_state.vals_size ; ++i)
+ {
req.start_state.vals[i] = m_basePos[i];
+ req.goal_state.vals[i] = m_basePos[i];
+ }
+
req.goal_state.vals[0] += 3.5;
req.allowed_time = 5.0;
@@ -104,7 +108,7 @@
req.threshold = 0.01;
req.distance_metric = "L2Square";
req.interpolate = 1;
- req.times = 100;
+ req.times = 1;
initialState(req.start_state);
@@ -130,7 +134,7 @@
req.threshold = 0.01;
req.distance_metric = "L2Square";
req.interpolate = 1;
- req.times = 100;
+ req.times = 1;
initialState(req.start_state);
Modified: pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-22 17:47:41 UTC (rev 3483)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-22 17:51:04 UTC (rev 3484)
@@ -112,9 +112,6 @@
m_displayRobot = true;
m_displayObstacles = true;
m_checkCollision = false;
-
- double sphere[3] = {0.7, 0.5, 0.7 };
- m_collisionSpace->addPointCloud(1, sphere, 0.2);
}
~PlanningWorldViewer(void)
Modified: pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp 2008-08-22 17:47:41 UTC (rev 3483)
+++ pkg/trunk/world_models/fake_world_3d_map/src/fake_world_3d_map.cpp 2008-08-22 17:51:04 UTC (rev 3484)
@@ -107,7 +107,10 @@
{
case 1:
oneObstacle();
- break;
+ break;
+ case 3:
+ planeX();
+ break;
default:
break;
}
@@ -122,11 +125,17 @@
void oneObstacle(void)
{
m_toPublish.set_pts_size(1);
- m_toPublish.pts[0].x = 0.0;
- m_toPublish.pts[0].y = 0.0;
- m_toPublish.pts[0].z = 0.0;
+ m_toPublish.pts[0].x = 0.6;
+ m_toPublish.pts[0].y = 0.35;
+ m_toPublish.pts[0].z = 0.75;
}
+ void planeX(void)
+ {
+ // for (int i = 0 ; i < 20 ; ++i)
+ // for (int j = 0 ; j < 20 ;
+ }
+
std_msgs::PointCloudFloat32 m_toPublish;
random_utils::rngState m_rng;
@@ -147,6 +156,7 @@
FakeWorld3DMap *map = new FakeWorld3DMap();
int index = 0;
sscanf(argv[1], "%d", &index);
+ sleep(1);
map->sendMap(index);
map->shutdown();
delete map;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-08-22 22:38:14
|
Revision: 3502
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3502&view=rev
Author: stuglaser
Date: 2008-08-22 22:38:24 +0000 (Fri, 22 Aug 2008)
Log Message:
-----------
Made the torso joint prismatic, and handled prismatic (slider) joints in the gazebo plugin.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-08-22 22:37:58 UTC (rev 3501)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/gazebo_actuators.cpp 2008-08-22 22:38:24 UTC (rev 3502)
@@ -141,11 +141,26 @@
{
if (!joints_[i])
continue;
- assert(joints_[i]->GetType() == Joint::HINGE);
- HingeJoint *hj = (HingeJoint*)joints_[i];
- fake_model_.joints_[i]->position_ = hj->GetAngle();
- fake_model_.joints_[i]->velocity_ = hj->GetAngleRate();
+
fake_model_.joints_[i]->applied_effort_ = fake_model_.joints_[i]->commanded_effort_;
+
+ switch(joints_[i]->GetType())
+ {
+ case Joint::HINGE: {
+ HingeJoint *hj = (HingeJoint*)joints_[i];
+ fake_model_.joints_[i]->position_ = hj->GetAngle();
+ fake_model_.joints_[i]->velocity_ = hj->GetAngleRate();
+ break;
+ }
+ case Joint::SLIDER: {
+ SliderJoint *sj = (SliderJoint*)joints_[i];
+ fake_model_.joints_[i]->position_ = sj->GetPosition();
+ fake_model_.joints_[i]->velocity_ = sj->GetPositionRate();
+ break;
+ }
+ default:
+ abort();
+ }
}
// Reverses the transmissions to propagate the joint position into the actuators.
@@ -171,9 +186,19 @@
{
if (!joints_[i])
continue;
- assert(joints_[i]->GetType() == Joint::HINGE);
- HingeJoint *hj = (HingeJoint*)joints_[i];
- hj->SetTorque(fake_model_.joints_[i]->commanded_effort_);
+
+ double effort = fake_model_.joints_[i]->commanded_effort_;
+ switch (joints_[i]->GetType())
+ {
+ case Joint::HINGE:
+ ((HingeJoint*)joints_[i])->SetTorque(effort);
+ break;
+ case Joint::SLIDER:
+ ((SliderJoint*)joints_[i])->SetSliderForce(effort);
+ break;
+ default:
+ abort();
+ }
}
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-08-22 22:37:58 UTC (rev 3501)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-08-22 22:38:24 UTC (rev 3502)
@@ -794,7 +794,7 @@
<joint name="shoulder_pan_left_joint" type="revolute" >
<insert_const_block name="pr2_shoulder_pan_joint"/>
</joint>
- <joint name="torso_joint">
+ <joint name="torso_joint" type="prismatic">
<limit min="0.0" max="torso_max_travel_range" effort="100" velocity="5" />
<axis xyz="0 0 1" />
</joint>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <MP...@us...> - 2008-08-22 22:43:17
|
Revision: 3505
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3505&view=rev
Author: MPPics
Date: 2008-08-22 22:43:27 +0000 (Fri, 22 Aug 2008)
Log Message:
-----------
Adding octrees to visualizer
Modified Paths:
--------------
pkg/trunk/vision/scan_utils/src/cloudToOctree/cloudToOctree.cpp
pkg/trunk/visualization/ogre_visualizer/manifest.xml
Added Paths:
-----------
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.cpp
pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h
Modified: pkg/trunk/vision/scan_utils/src/cloudToOctree/cloudToOctree.cpp
===================================================================
--- pkg/trunk/vision/scan_utils/src/cloudToOctree/cloudToOctree.cpp 2008-08-22 22:40:25 UTC (rev 3504)
+++ pkg/trunk/vision/scan_utils/src/cloudToOctree/cloudToOctree.cpp 2008-08-22 22:43:27 UTC (rev 3505)
@@ -30,9 +30,9 @@
CloudToOctree(float cellSize);
~CloudToOctree();
void resetOctree();
- };
-
-
+ };
+
+
CloudToOctree::CloudToOctree(float cellSize) : ros::node("cloud_to_octree_node")
{
/* initialize Octree to depth 0. This means that the
@@ -46,7 +46,7 @@
//subsribe and advertise to relevant ROS topics
subscribe("full_cloud", mNewCloud, &CloudToOctree::fullCloudCallback,1);
- advertise<OctreeMsg>("full_octree",1);
+ advertise<OctreeMsg>("full_octree",1);
fprintf(stderr,"ROS cloud-to-octree node with cell size %f created and subscribed!\n",cellSize);
}
@@ -56,6 +56,7 @@
}
void CloudToOctree::fullCloudCallback() {
+ fprintf(stderr,"Cloud received with %d points!\n",mNewCloud.get_pts_size());
if ( mNewCloud.get_pts_size() == 0 ) {
return;
}
@@ -68,7 +69,7 @@
OctreeMsg outMsg;
mOctree->getAsMsg(outMsg);
//and publish
- publish("full_octree",outMsg);
+ publish("full_octree",outMsg);
}
/*! Clears the contents of the Octree. Note that the size and
depth are not changed, they remain whatever they were
@@ -80,7 +81,7 @@
2*cell_size along each dimension.
*/
void CloudToOctree::resetOctree() {
- mOctree->clear();
+ mOctree->clear();
}
} //namespace scan_utils
Modified: pkg/trunk/visualization/ogre_visualizer/manifest.xml
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/manifest.xml 2008-08-22 22:40:25 UTC (rev 3504)
+++ pkg/trunk/visualization/ogre_visualizer/manifest.xml 2008-08-22 22:43:27 UTC (rev 3505)
@@ -15,6 +15,7 @@
<depend package="rosTF"/>
<depend package="laser_scan_utils"/>
<depend package="wg_robot_description_parser"/>
+ <depend package="scan_utils" />
<depend package="wxpropgrid"/>
<depend package="wx_topic_display"/>
<export>
Added: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.cpp
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.cpp (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.cpp 2008-08-22 22:43:27 UTC (rev 3505)
@@ -0,0 +1,174 @@
+/*
+ * 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, Inc. 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.
+ */
+
+/*
+ * octree_visualizer.cpp
+ *
+ * Created on: Aug 20, 2008
+ * Author: Matthew Piccoli and Matei Ciocarlie
+ */
+
+#include "octree_visualizer.h"
+#include "octree.h"
+#include "dataTypes.h"
+#include "ros/node.h"
+#include <rosTF/rosTF.h>
+
+#include "../common.h"
+
+namespace ogre_vis
+{
+
+ OctreeVisualizer::OctreeVisualizer(Ogre::SceneManager* sceneManager, ros::node* node, rosTFClient* tfClient, const std::string& name, bool enabled )
+ : VisualizerBase( sceneManager, node, tfClient, name, enabled )
+ , m_R( 1.0 )
+ , m_G( 0.0 )
+ , m_B( 0.0 )
+ , m_NewMessage( false )
+ {
+ static uint32_t count = 0;
+ std::stringstream ss;
+ ss << "Cone" << count++;
+
+ m_ManualObject = m_SceneManager->createManualObject( ss.str() );
+
+ m_SceneNode = m_SceneManager->getRootSceneNode()->createChildSceneNode();
+ m_SceneNode->attachObject( m_ManualObject );
+
+ if ( IsEnabled() )
+ {
+ OnEnable();
+ }
+
+ }
+
+ OctreeVisualizer::~OctreeVisualizer() {
+ // TODO Auto-generated destructor stub
+ }
+
+ void OctreeVisualizer::OnEnable()
+ {
+ m_ManualObject->setVisible( true );
+ Subscribe();
+ }
+
+ void OctreeVisualizer::OnDisable()
+ {
+ m_ManualObject->setVisible( false );
+ Unsubscribe();
+ }
+
+ void OctreeVisualizer::Subscribe()
+ {
+ if ( !IsEnabled() )
+ {
+ return;
+ }
+
+ if ( !m_OctreeTopic.empty() )
+ {
+ m_ROSNode->subscribe( m_OctreeTopic, m_OctreeMessage, &OctreeVisualizer::IncomingOctreeCallback, this, 10 );
+ }
+ }
+
+ void OctreeVisualizer::Unsubscribe()
+ {
+ if ( !m_OctreeTopic.empty() )
+ {
+ m_ROSNode->unsubscribe( m_OctreeTopic );
+ }
+ }
+
+ void OctreeVisualizer::Update( float dt )
+ {
+ m_OctreeMessage.lock();
+
+ if ( m_NewMessage )
+ {
+ scan_utils::Octree<char> octree(0,0,0,0,0,0,1,0);
+ octree.setFromMsg(m_OctreeMessage);
+
+ std::list<scan_utils::Triangle> triangles;
+ octree.getAllTriangles(triangles);
+
+ std::list<scan_utils::Triangle>::iterator it;
+ Ogre::Vector3 v1, v2, v3;
+
+ m_ManualObject->clear();
+ m_ManualObject->begin( m_MaterialName, Ogre::RenderOperation::OT_TRIANGLE_LIST );
+
+ int i = 0;
+ for ( it = triangles.begin(); it!=triangles.end(); it++, i += 3)
+ {
+ v1 = Ogre::Vector3( it->p1.x, it->p1.y, it->p1.z );
+ RobotToOgre( v1 );
+ m_ManualObject->position( v1 );
+ m_ManualObject->colour( m_R, m_G, m_B );
+ v2 = Ogre::Vector3( it->p2.x, it->p2.y, it->p2.z );
+ RobotToOgre( v2 );
+ m_ManualObject->position( v2 );
+ m_ManualObject->colour( m_R, m_G, m_B );
+ v3 = Ogre::Vector3( it->p3.x, it->p3.y, it->p3.z );
+ RobotToOgre( v3 );
+ m_ManualObject->position( v3 );
+ m_ManualObject->colour( m_R, m_G, m_B );
+
+ //m_ManualObject->triangle( i, i + 2, i + 1 );
+ }
+ m_ManualObject->end();
+
+ m_NewMessage = false;
+ }
+
+ m_OctreeMessage.unlock();
+ }
+
+ void OctreeVisualizer::IncomingOctreeCallback()
+ {
+ m_OctreeMessage.lock();
+ m_NewMessage = true;
+ m_OctreeMessage.unlock();
+ }
+
+ void OctreeVisualizer::SetOctreeTopic( const std::string& topic )
+ {
+ Unsubscribe();
+
+ m_OctreeTopic = topic;
+
+ Subscribe();
+ }
+
+ void OctreeVisualizer::SetColor( float r, float g, float b )
+ {
+ m_R = r;
+ m_G = g;
+ m_B = b;
+ }
+}
Added: pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h
===================================================================
--- pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h (rev 0)
+++ pkg/trunk/visualization/ogre_visualizer/src/ogre_visualizer/visualizers/octree_visualizer.h 2008-08-22 22:43:27 UTC (rev 3505)
@@ -0,0 +1,83 @@
+/*
+ * 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, Inc. 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.
+ */
+
+/*
+ * octree_visualizer.h
+ *
+ * Created on: Aug 20, 2008
+ * Author: Matthew Piccoli and Matei Ciocarlie
+ */
+
+#ifndef OCTREE_VISUALIZER_H_
+#define OCTREE_VISUALIZER_H_
+
+#include "../visualizer_base.h"
+#include <scan_utils/OctreeMsg.h>
+
+#include <Ogre.h>
+namespace ogre_vis
+{
+
+ class OctreeVisualizer : public VisualizerBase
+ {
+ public:
+ OctreeVisualizer( Ogre::SceneManager* sceneManager, ros::node* node, rosTFClient* tfClient, const std::string& name, bool enabled );
+ virtual ~OctreeVisualizer();
+
+ void SetOctreeTopic( const std::string& topic );
+ void SetColor( float r, float g, float b );
+
+ virtual void Update( float dt );
+
+ protected:
+ // overrides from VisualizerBase
+ virtual void OnEnable();
+ virtual void OnDisable();
+ void Subscribe();
+ void Unsubscribe();
+
+ void IncomingOctreeCallback();
+
+ float m_R;
+ float m_G;
+ float m_B;
+
+ Ogre::SceneNode* m_SceneNode;
+ Ogre::ManualObject* m_ManualObject;
+ Ogre::MaterialPtr m_Material;
+ std::string m_MaterialName;
+ std::string m_OctreeTopic;
+
+ scan_utils::OctreeMsg m_OctreeMessage;
+
+ bool m_NewMessage;
+ };
+
+}
+#endif /* OCTREE_VISUALIZER_H_ */
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-23 00:07:28
|
Revision: 3520
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3520&view=rev
Author: isucan
Date: 2008-08-23 00:07:36 +0000 (Sat, 23 Aug 2008)
Log Message:
-----------
reorganized kinematic planning node. we now take into account requested planners and distance metrics
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/robot_srvs/srv/KinematicPlanLinkPosition.srv
pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
Added Paths:
-----------
pkg/trunk/motion_planning/kinematic_planning/include/KinematicPlanningXMLModel.h
pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg
Removed Paths:
-------------
pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h
Added: pkg/trunk/motion_planning/kinematic_planning/include/KinematicPlanningXMLModel.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/KinematicPlanningXMLModel.h (rev 0)
+++ pkg/trunk/motion_planning/kinematic_planning/include/KinematicPlanningXMLModel.h 2008-08-23 00:07:36 UTC (rev 3520)
@@ -0,0 +1,194 @@
+/*********************************************************************
+* 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 <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
+#include <collision_space/environment.h>
+#include <planning_models/kinematic.h>
+
+#include <vector>
+#include <string>
+#include <map>
+
+struct KinematicPlannerSetup
+{
+ ompl::Planner_t mp;
+ ompl::SpaceInformationKinematic_t si;
+ ompl::SpaceInformation::StateValidityChecker_t svc;
+ std::map<std::string, ompl::SpaceInformation::StateDistanceEvaluator_t> sde;
+ ompl::PathSmootherKinematic_t smoother;
+};
+
+struct XMLModel
+{
+ XMLModel(void)
+ {
+ groupID = -1;
+ collisionSpaceID = 0;
+ collisionSpace = NULL;
+ }
+
+ ~XMLModel(void)
+ {
+ for (std::map<std::string, KinematicPlannerSetup>::iterator i = planners.begin(); i != planners.end() ; ++i)
+ {
+ delete i->second.mp;
+ delete i->second.svc;
+ for (std::map<std::string, ompl::SpaceInformation::StateDistanceEvaluator_t>::iterator j = i->second.sde.begin(); j != i->second.sde.end() ; ++j)
+ delete j->second;
+ delete i->second.smoother;
+ delete i->second.si;
+ }
+ }
+
+ std::map<std::string, KinematicPlannerSetup> planners;
+ collision_space::EnvironmentModel *collisionSpace;
+ unsigned int collisionSpaceID;
+ planning_models::KinematicModel *kmodel;
+ std::string groupName;
+ int groupID;
+};
+
+
+/** This class configures an instance of SpaceInformationKinematic with data from a KinematicModel */
+class SpaceInformationXMLModel : public ompl::SpaceInformationKinematic
+{
+ public:
+ SpaceInformationXMLModel(planning_models::KinematicModel *kmodel, int groupID = -1, double divisions = 20.0) : SpaceInformationKinematic()
+ {
+ m_kmodel = kmodel;
+ m_groupID = groupID;
+ m_divisions = divisions;
+
+ /* compute the state space for this group */
+ m_stateDimension = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID].size() : m_kmodel->stateDimension;
+ m_stateComponent.resize(m_stateDimension);
+
+ for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
+ {
+ if (m_stateComponent[i].type == StateComponent::UNKNOWN)
+ m_stateComponent[i].type = StateComponent::NORMAL;
+ int p = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID][i] * 2 : i * 2;
+ m_stateComponent[i].minValue = m_kmodel->stateBounds[p ];
+ m_stateComponent[i].maxValue = m_kmodel->stateBounds[p + 1];
+ m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / m_divisions;
+
+ for (unsigned int j = 0 ; j < m_kmodel->floatingJoints.size() ; ++j)
+ if (m_kmodel->floatingJoints[j] == p)
+ {
+ m_floatingJoints.push_back(i);
+ m_stateComponent[i + 3].type = StateComponent::QUATERNION;
+ break;
+ }
+
+ for (unsigned int j = 0 ; j < m_kmodel->planarJoints.size() ; ++j)
+ if (m_kmodel->planarJoints[j] == p)
+ {
+ m_planarJoints.push_back(i);
+ break;
+ }
+ }
+ updateResolution();
+ }
+
+ virtual ~SpaceInformationXMLModel(void)
+ {
+ }
+
+ /** For planar and floating joints, we have infinite
+ dimensions. The bounds for these dimensions are set by the
+ user. */
+ void setPlanningVolume(double x0, double y0, double z0, double x1, double y1, double z1)
+ {
+ for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
+ {
+ int id = m_floatingJoints[i];
+ m_stateComponent[id ].minValue = x0;
+ m_stateComponent[id ].maxValue = x1;
+ m_stateComponent[id + 1].minValue = y0;
+ m_stateComponent[id + 1].maxValue = y1;
+ m_stateComponent[id + 2].minValue = z0;
+ m_stateComponent[id + 2].maxValue = z1;
+ for (int j = 0 ; j < 3 ; ++j)
+ m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
+ }
+ updateResolution();
+ }
+
+ void setPlanningArea(double x0, double y0, double x1, double y1)
+ {
+ for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
+ {
+ int id = m_planarJoints[i];
+ m_stateComponent[id ].minValue = x0;
+ m_stateComponent[id ].maxValue = x1;
+ m_stateComponent[id + 1].minValue = y0;
+ m_stateComponent[id + 1].maxValue = y1;
+ for (int j = 0 ; j < 2 ; ++j)
+ m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
+ }
+ updateResolution();
+ }
+
+ protected:
+
+ void updateResolution(void)
+ {
+ /* for movement in plane/space, we want to make sure the resolution is small enough */
+ for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
+ {
+ if (m_stateComponent[m_planarJoints[i]].resolution > 0.1)
+ m_stateComponent[m_planarJoints[i]].resolution = 0.1;
+ if (m_stateComponent[m_planarJoints[i] + 1].resolution > 0.1)
+ m_stateComponent[m_planarJoints[i] + 1].resolution = 0.1;
+ }
+ for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
+ {
+ if (m_stateComponent[m_floatingJoints[i]].resolution > 0.1)
+ m_stateComponent[m_floatingJoints[i]].resolution = 0.1;
+ if (m_stateComponent[m_floatingJoints[i] + 1].resolution > 0.1)
+ m_stateComponent[m_floatingJoints[i] + 1].resolution = 0.1;
+ if (m_stateComponent[m_floatingJoints[i] + 2].resolution > 0.1)
+ m_stateComponent[m_floatingJoints[i] + 2].resolution = 0.1;
+ }
+ }
+
+ double m_divisions;
+ planning_models::KinematicModel *m_kmodel;
+ int m_groupID;
+ std::vector<int> m_floatingJoints;
+ std::vector<int> m_planarJoints;
+
+};
Deleted: pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h 2008-08-22 23:54:37 UTC (rev 3519)
+++ pkg/trunk/motion_planning/kinematic_planning/include/SpaceInformationXMLModel.h 2008-08-23 00:07:36 UTC (rev 3520)
@@ -1,146 +0,0 @@
-/*********************************************************************
-* 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 <ompl/extension/samplingbased/kinematic/SpaceInformationKinematic.h>
-#include <planning_models/kinematic.h>
-
-/** This class configures an instance of SpaceInformationKinematic with data from a KinematicModel */
-class SpaceInformationXMLModel : public ompl::SpaceInformationKinematic
-{
- public:
- SpaceInformationXMLModel(planning_models::KinematicModel *kmodel, int groupID = -1, double divisions = 20.0) : SpaceInformationKinematic()
- {
- m_kmodel = kmodel;
- m_groupID = groupID;
- m_divisions = divisions;
-
- m_stateDimension = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID].size() : m_kmodel->stateDimension;
- m_stateComponent.resize(m_stateDimension);
-
- for (unsigned int i = 0 ; i < m_stateDimension ; ++i)
- {
- if (m_stateComponent[i].type == StateComponent::UNKNOWN)
- m_stateComponent[i].type = StateComponent::NORMAL;
- int p = m_groupID >= 0 ? m_kmodel->groupStateIndexList[m_groupID][i] * 2 : i * 2;
- m_stateComponent[i].minValue = m_kmodel->stateBounds[p ];
- m_stateComponent[i].maxValue = m_kmodel->stateBounds[p + 1];
- m_stateComponent[i].resolution = (m_stateComponent[i].maxValue - m_stateComponent[i].minValue) / m_divisions;
-
- for (unsigned int j = 0 ; j < m_kmodel->floatingJoints.size() ; ++j)
- if (m_kmodel->floatingJoints[j] == p)
- {
- m_floatingJoints.push_back(i);
- m_stateComponent[i + 3].type = StateComponent::QUATERNION;
- break;
- }
-
- for (unsigned int j = 0 ; j < m_kmodel->planarJoints.size() ; ++j)
- if (m_kmodel->planarJoints[j] == p)
- {
- m_planarJoints.push_back(i);
- break;
- }
- }
- updateResolution();
- }
-
- virtual ~SpaceInformationXMLModel(void)
- {
- }
-
-
- void setPlanningVolume(double x0, double y0, double z0, double x1, double y1, double z1)
- {
- for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
- {
- int id = m_floatingJoints[i];
- m_stateComponent[id ].minValue = x0;
- m_stateComponent[id ].maxValue = x1;
- m_stateComponent[id + 1].minValue = y0;
- m_stateComponent[id + 1].maxValue = y1;
- m_stateComponent[id + 2].minValue = z0;
- m_stateComponent[id + 2].maxValue = z1;
- for (int j = 0 ; j < 3 ; ++j)
- m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
- }
- updateResolution();
- }
-
- void setPlanningArea(double x0, double y0, double x1, double y1)
- {
- for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
- {
- int id = m_planarJoints[i];
- m_stateComponent[id ].minValue = x0;
- m_stateComponent[id ].maxValue = x1;
- m_stateComponent[id + 1].minValue = y0;
- m_stateComponent[id + 1].maxValue = y1;
- for (int j = 0 ; j < 2 ; ++j)
- m_stateComponent[j + id].resolution = (m_stateComponent[j + id].maxValue - m_stateComponent[j + id].minValue) / m_divisions;
- }
- updateResolution();
- }
-
- protected:
-
- void updateResolution(void)
- {
- /* for movement in plane/space, we want to make sure the resolution is small enough */
- for (unsigned int i = 0 ; i < m_planarJoints.size() ; ++i)
- {
- if (m_stateComponent[m_planarJoints[i]].resolution > 0.1)
- m_stateComponent[m_planarJoints[i]].resolution = 0.1;
- if (m_stateComponent[m_planarJoints[i] + 1].resolution > 0.1)
- m_stateComponent[m_planarJoints[i] + 1].resolution = 0.1;
- }
- for (unsigned int i = 0 ; i < m_floatingJoints.size() ; ++i)
- {
- if (m_stateComponent[m_floatingJoints[i]].resolution > 0.1)
- m_stateComponent[m_floatingJoints[i]].resolution = 0.1;
- if (m_stateComponent[m_floatingJoints[i] + 1].resolution > 0.1)
- m_stateComponent[m_floatingJoints[i] + 1].resolution = 0.1;
- if (m_stateComponent[m_floatingJoints[i] + 2].resolution > 0.1)
- m_stateComponent[m_floatingJoints[i] + 2].resolution = 0.1;
- }
- }
-
- double m_divisions;
- planning_models::KinematicModel *m_kmodel;
- int m_groupID;
- std::vector<int> m_floatingJoints;
- std::vector<int> m_planarJoints;
-
-};
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-22 23:54:37 UTC (rev 3519)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-23 00:07:36 UTC (rev 3520)
@@ -95,7 +95,7 @@
#include <ompl/extension/samplingbased/kinematic/extension/rrt/LazyRRT.h>
#include <ompl/extension/samplingbased/kinematic/extension/rrt/RRT.h>
-#include "SpaceInformationXMLModel.h"
+#include "KinematicPlanningXMLModel.h"
#include <profiling_utils/profiler.h>
#include <string_utils/string_utils.h>
@@ -113,105 +113,151 @@
KinematicPlanning(const std::string &robot_model) : ros::node("kinematic_planning"),
planning_node_util::NodeCollisionModel(dynamic_cast<ros::node*>(this),
- robot_model,
- new collision_space::EnvironmentModelODE())
+ robot_model)
{
- advertise_service("plan_kinematic_path_state", &KinematicPlanning::plan);
+ advertise_service("plan_kinematic_path_state", &KinematicPlanning::planToState);
}
- ~KinematicPlanning(void)
+ virtual ~KinematicPlanning(void)
{
- for (std::map<std::string, Model*>::iterator i = m_models.begin() ; i != m_models.end() ; i++)
+ for (std::map<std::string, XMLModel*>::iterator i = m_models.begin() ; i != m_models.end() ; i++)
delete i->second;
- }
+ }
- bool plan(robot_srvs::KinematicPlanState::request &req, robot_srvs::KinematicPlanState::response &res)
+ bool areSpaceParamsValid(robot_msgs::KinematicSpaceParameters ¶ms)
{
- if (m_models.find(req.model_id) == m_models.end())
+ if (m_models.find(params.model_id) == m_models.end())
{
- std::cerr << "Cannot plan for '" << req.model_id << "'. Model does not exist" << std::endl;
+ std::cerr << "Cannot plan for '" << params.model_id << "'. Model does not exist" << std::endl;
return false;
}
+
+ /* find the model */
+ XMLModel *m = m_models[params.model_id];
- Model *m = m_models[req.model_id];
- Planner &p = m->planners[0];
+ /* if the user did not specify a planner, use the first available one */
+ if (params.planner_id.empty())
+ {
+ if (!m->planners.empty())
+ params.planner_id = m->planners.begin()->first;
+ }
- if (m->kmodel->stateDimension != req.start_state.get_vals_size())
+ /* check if desired planner exists */
+ std::map<std::string, KinematicPlannerSetup>::iterator plannerIt = m->planners.find(params.planner_id);
+ if (plannerIt == m->planners.end())
{
- std::cerr << "Dimension of start state expected to be " << m->kmodel->stateDimension << " but was received as " << req.start_state.get_vals_size() << std::endl;
- return false;
+ std::cerr << "Motion planner not found: '" << params.planner_id << "'" << std::endl;
+ return false;
}
- const int dim = req.goal_state.get_vals_size();
- if ((int)p.si->getStateDimension() != dim)
+ KinematicPlannerSetup &psetup = plannerIt->second;
+
+ /* check if the desired distance metric is defined */
+ if (psetup.sde.find(params.distance_metric) == psetup.sde.end())
{
- std::cerr << "Dimension of start goal expected to be " << p.si->getStateDimension() << " but was received as " << dim << std::endl;
+ std::cerr << "Distance evaluator not found: '" << params.distance_metric << "'" << std::endl;
return false;
}
+ return true;
+ }
+
+ bool isRequestValid(robot_srvs::KinematicPlanState::request &req)
+ {
+ if (!areSpaceParamsValid(req.params))
+ return false;
+
+ XMLModel *m = m_models[req.params.model_id];
+ KinematicPlannerSetup &psetup = m->planners[req.params.planner_id];
- if (req.times <= 0)
+ if (m->kmodel->stateDimension != req.start_state.get_vals_size())
{
- std::cerr << "Request specifies motion plan should be computed " << req.times << " times" << std::endl;
+ std::cerr << "Dimension of start state expected to be " << m->kmodel->stateDimension << " but was received as " << req.start_state.get_vals_size() << std::endl;
return false;
}
-
+ if (psetup.si->getStateDimension() != req.goal_state.get_vals_size())
+ {
+ std::cerr << "Dimension of start goal expected to be " << psetup.si->getStateDimension() << " but was received as " << req.goal_state.get_vals_size() << std::endl;
+ return false;
+ }
+
+ return true;
+ }
+
+ void setupStateSpaceAndStartState(XMLModel *m, KinematicPlannerSetup &p,
+ robot_msgs::KinematicSpaceParameters ¶ms,
+ robot_msgs::KinematicState &start_state)
+ {
/* set the workspace volume for planning */
// only area or volume should go through... not sure what happens on really complicated models where
// we have both multiple planar and multiple floating joints...
- static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningArea(req.volumeMin.x, req.volumeMin.y,
- req.volumeMax.x, req.volumeMax.y);
- static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningVolume(req.volumeMin.x, req.volumeMin.y, req.volumeMin.z,
- req.volumeMax.x, req.volumeMax.y, req.volumeMax.z);
-
+ static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningArea(params.volumeMin.x, params.volumeMin.y,
+ params.volumeMax.x, params.volumeMax.y);
+ static_cast<SpaceInformationXMLModel*>(p.si)->setPlanningVolume(params.volumeMin.x, params.volumeMin.y, params.volumeMin.z,
+ params.volumeMax.x, params.volumeMax.y, params.volumeMax.z);
+
+ p.si->setStateDistanceEvaluator(p.sde[params.distance_metric]);
+
/* set the starting state */
- ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
-
+ const unsigned int dim = p.si->getStateDimension();
+ ompl::SpaceInformationKinematic::StateKinematic_t start = new ompl::SpaceInformationKinematic::StateKinematic(dim);
+
if (m->groupID >= 0)
{
/* set the pose of the whole robot */
- m->kmodel->computeTransforms(req.start_state.vals);
+ m->kmodel->computeTransforms(start_state.vals);
m->collisionSpace->updateRobotModel(m->collisionSpaceID);
/* extract the components needed for the start state of the desired group */
- for (int i = 0 ; i < dim ; ++i)
- start->values[i] = req.start_state.vals[m->kmodel->groupStateIndexList[m->groupID][i]];
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ start->values[i] = start_state.vals[m->kmodel->groupStateIndexList[m->groupID][i]];
}
else
{
/* the start state is the complete state */
- for (int i = 0 ; i < dim ; ++i)
- start->values[i] = req.start_state.vals[i];
+ for (unsigned int i = 0 ; i < dim ; ++i)
+ start->values[i] = start_state.vals[i];
}
p.si->addStartState(start);
-
+ }
+
+ void setupGoalState(KinematicPlannerSetup &p, robot_srvs::KinematicPlanState::request &req)
+ {
/* set the goal */
ompl::SpaceInformationKinematic::GoalStateKinematic_t goal = new ompl::SpaceInformationKinematic::GoalStateKinematic(p.si);
+ const unsigned int dim = p.si->getStateDimension();
goal->state = new ompl::SpaceInformationKinematic::StateKinematic(dim);
- for (int i = 0 ; i < dim ; ++i)
+ for (unsigned int i = 0 ; i < dim ; ++i)
goal->state->values[i] = req.goal_state.vals[i];
goal->threshold = req.threshold;
p.si->setGoal(goal);
+ }
+
+ void computePlan(KinematicPlannerSetup &p, int times, double allowed_time, bool interpolate,
+ ompl::SpaceInformationKinematic::PathKinematic_t &bestPath, double &bestDifference)
+ {
+
+ if (times <= 0)
+ {
+ std::cerr << "Request specifies motion plan cannot be computed " << times << " times" << std::endl;
+ return;
+ }
- printf("=======================================\n");
- p.si->printSettings();
- printf("=======================================\n");
+ /* do the planning */
+ bestPath = NULL;
+ bestDifference = 0.0;
+ double totalTime = 0.0;
+ ompl::SpaceInformation::Goal_t goal = p.si->getGoal();
-
- /* do the planning */
- ompl::SpaceInformationKinematic::PathKinematic_t bestPath = NULL;
- double bestDifference = 0.0;
- double totalTime = 0.0;
-
m_collisionSpace->lock();
profiling_utils::Profiler::Start();
- for (int i = 0 ; i < req.times ; ++i)
+ for (int i = 0 ; i < times ; ++i)
{
ros::Time startTime = ros::Time::now();
- bool ok = p.mp->solve(req.allowed_time);
+ bool ok = p.mp->solve(allowed_time);
double tsolve = (ros::Time::now() - startTime).to_double();
std::cout << (ok ? "[Success]" : "[Failure]") << " Motion planner spent " << tsolve << " seconds" << std::endl;
totalTime += tsolve;
@@ -224,7 +270,7 @@
p.smoother->smoothVertices(path);
double tsmooth = (ros::Time::now() - startTime).to_double();
std::cout << " Smoother spent " << tsmooth << " seconds (" << (tsmooth + tsolve) << " seconds in total)" << std::endl;
- if (req.interpolate)
+ if (interpolate)
p.si->interpolatePath(path);
if (bestPath == NULL || bestDifference > goal->getDifference() ||
(bestPath && bestDifference == goal->getDifference() && bestPath->states.size() > path->states.size()))
@@ -241,34 +287,74 @@
}
profiling_utils::Profiler::Stop();
+ m_collisionSpace->unlock();
- m_collisionSpace->unlock();
+ std::cout << std::endl << "Total planning time: " << totalTime << "; Average planning time: " << (totalTime / (double)times) << " (seconds)" << std::endl;
+ }
- std::cout << std::endl << "Total planning time: " << totalTime << "; Average planning time: " << (totalTime / (double)req.times) << " (seconds)" << std::endl;
-
+ void fillSolution(KinematicPlannerSetup &p, ompl::SpaceInformationKinematic::PathKinematic_t bestPath, double bestDifference,
+ robot_msgs::KinematicPath &path, double &distance)
+ {
+ const unsigned int dim = p.si->getStateDimension();
+
/* copy the solution to the result */
if (bestPath)
{
- res.path.set_states_size(bestPath->states.size());
+ path.set_states_size(bestPath->states.size());
for (unsigned int i = 0 ; i < bestPath->states.size() ; ++i)
{
- res.path.states[i].set_vals_size(dim);
- for (int j = 0 ; j < dim ; ++j)
- res.path.states[i].vals[j] = bestPath->states[i]->values[j];
+ path.states[i].set_vals_size(dim);
+ for (unsigned int j = 0 ; j < dim ; ++j)
+ path.states[i].vals[j] = bestPath->states[i]->values[j];
}
- res.distance = bestDifference;
+ distance = bestDifference;
delete bestPath;
}
else
{
- res.path.set_states_size(0);
- res.distance = -1.0;
+ path.set_states_size(0);
+ distance = -1.0;
}
-
+ }
+
+ void cleanupPlanningData(KinematicPlannerSetup &p)
+ {
/* cleanup */
p.si->clearGoal();
- p.si->clearStartStates();
+ p.si->clearStartStates();
+ }
+
+ bool planToState(robot_srvs::KinematicPlanState::request &req, robot_srvs::KinematicPlanState::response &res)
+ {
+ if (!isRequestValid(req))
+ return false;
+ /* find the data we need */
+ XMLModel *m = m_models[req.params.model_id];
+ KinematicPlannerSetup &psetup = m->planners[req.params.planner_id];
+
+ /* configure state space and starting state */
+ setupStateSpaceAndStartState(m, psetup, req.params, req.start_state);
+
+ /* add goal state */
+ setupGoalState(psetup, req);
+
+ /* print some information */
+ printf("=======================================\n");
+ psetup.si->printSettings();
+ printf("=======================================\n");
+
+ /* compute actual motion plan */
+ ompl::SpaceInformationKinematic::PathKinematic_t bestPath = NULL;
+ double bestDifference = 0.0;
+ computePlan(psetup, req.times, req.allowed_time, req.interpolate, bestPath, bestDifference);
+
+ /* fill in the results */
+ fillSolution(psetup, bestPath, bestDifference, res.path, res.distance);
+
+ /* clear memory */
+ cleanupPlanningData(psetup);
+
return true;
}
@@ -278,7 +364,7 @@
defaultPosition();
/* set the data for the model */
- Model *model = new Model();
+ XMLModel *model = new XMLModel();
model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
@@ -298,7 +384,7 @@
for (unsigned int i = 0 ; i < groups.size() ; ++i)
{
- Model *model = new Model();
+ XMLModel *model = new XMLModel();
model->collisionSpaceID = 0;
model->collisionSpace = m_collisionSpace;
model->kmodel = m_kmodel;
@@ -311,87 +397,58 @@
void knownModels(std::vector<std::string> &model_ids)
{
- for (std::map<std::string, Model*>::const_iterator i = m_models.begin() ; i != m_models.end() ; ++i)
+ for (std::map<std::string, XMLModel*>::const_iterator i = m_models.begin() ; i != m_models.end() ; ++i)
model_ids.push_back(i->first);
}
private:
- class StateValidityPredicate;
-
- struct Planner
- {
- ompl::Planner_t mp;
- ompl::SpaceInformationKinematic_t si;
- StateValidityPredicate* svc;
- ompl::PathSmootherKinematic_t smoother;
- };
-
- struct Model
- {
- Model(void)
- {
- groupID = -1;
- collisionSpaceID = 0;
- collisionSpace = NULL;
- }
-
- ~Model(void)
- {
- for (unsigned int i = 0 ; i < planners.size() ; ++i)
- {
- delete planners[i].mp;
- delete planners[i].svc;
- delete planners[i].smoother;
- delete planners[i].si;
- }
- }
-
- std::vector<Planner> planners;
- collision_space::EnvironmentModel *collisionSpace;
- unsigned int collisionSpaceID;
- planning_models::KinematicModel *kmodel;
- std::string groupName;
- int groupID;
- };
-
class StateValidityPredicate : public ompl::SpaceInformation::StateValidityChecker
{
public:
- StateValidityPredicate(Model *model) : ompl::SpaceInformation::StateValidityChecker()
+ StateValidityPredicate(XMLModel *model) : ompl::SpaceInformation::StateValidityChecker()
{
m_model = model;
+ m_kc = NULL;
}
virtual bool operator()(const ompl::SpaceInformation::State_t state)
{
m_model->kmodel->computeTransforms(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
-
+
bool collision = m_model->collisionSpace->isCollision(m_model->collisionSpa...
[truncated message content] |
|
From: <is...@us...> - 2008-08-25 17:18:31
|
Revision: 3549
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3549&view=rev
Author: isucan
Date: 2008-08-25 17:18:32 +0000 (Mon, 25 Aug 2008)
Log Message:
-----------
more moving things around. the interface should now be stable.. .only internals of kinematic_planning will change
Modified Paths:
--------------
pkg/trunk/motion_planning/kinematic_planning/include/KinematicConstraintEvaluator.h
pkg/trunk/motion_planning/kinematic_planning/include/RequestPlan.h
pkg/trunk/motion_planning/kinematic_planning/include/RequestPlanLinkPosition.h
pkg/trunk/motion_planning/kinematic_planning/include/StateValidityChecker.h
pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
Added Paths:
-----------
pkg/trunk/robot_msgs/msg/KinematicConstraints.msg
pkg/trunk/robot_msgs/msg/PoseConstraint.msg
Removed Paths:
-------------
pkg/trunk/robot_msgs/msg/KinematicConstraint.msg
Modified: pkg/trunk/motion_planning/kinematic_planning/include/KinematicConstraintEvaluator.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/KinematicConstraintEvaluator.h 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/motion_planning/kinematic_planning/include/KinematicConstraintEvaluator.h 2008-08-25 17:18:32 UTC (rev 3549)
@@ -38,45 +38,82 @@
#define KINEMATIC_PLANNING_KINEMATIC_CONSTRAINT_EVALUATOR_
#include <planning_models/kinematic.h>
-#include <robot_msgs/KinematicConstraint.h>
+#include <robot_msgs/KinematicConstraints.h>
#include <iostream>
#include <vector>
+
class KinematicConstraintEvaluator
{
public:
KinematicConstraintEvaluator(void)
{
+ }
+
+ virtual ~KinematicConstraintEvaluator(void)
+ {
+ }
+
+ virtual void clear(void) = 0;
+ virtual void use(planning_models::KinematicModel *kmodel, const ros::msg *kc) = 0;
+ virtual bool decide(void) = 0;
+ virtual void print(std::ostream &out = std::cout) const
+ {
+ }
+};
+
+
+class PoseConstraintEvaluator : public KinematicConstraintEvaluator
+{
+ public:
+
+ PoseConstraintEvaluator(void) : KinematicConstraintEvaluator()
+ {
m_link = NULL;
}
- void use(planning_models::KinematicModel *kmodel, const robot_msgs::KinematicConstraint &kc)
+ virtual void use(planning_models::KinematicModel *kmodel, const ros::msg *kc)
{
- m_link = kmodel->getLink(kc.robot_link);
- m_kc = kc;
+ const robot_msgs::PoseConstraint *pc = dynamic_cast<const robot_msgs::PoseConstraint*>(kc);
+ if (pc)
+ use(kmodel, *pc);
}
- void clear(void)
+ void use(planning_models::KinematicModel *kmodel, const robot_msgs::PoseConstraint &pc)
{
+ m_link = kmodel->getLink(pc.robot_link);
+ m_pc = pc;
+ }
+
+ virtual void clear(void)
+ {
m_link = NULL;
}
+ virtual bool decide(void)
+ {
+ double dPos, dAng;
+ evaluate(&dPos, &dAng);
+
+ return decide(dPos, dAng);
+ }
+
void evaluate(double *distPos, double *distAng)
{
if (m_link)
{
- switch (m_kc.type)
+ switch (m_pc.type)
{
- case robot_msgs::KinematicConstraint::ONLY_POSITION:
+ case robot_msgs::PoseConstraint::ONLY_POSITION:
if (distPos)
{
libTF::Pose3D::Position bodyPos;
m_link->globalTrans.getPosition(bodyPos);
- double dx = bodyPos.x - m_kc.pose.position.x;
- double dy = bodyPos.y - m_kc.pose.position.y;
- double dz = bodyPos.z - m_kc.pose.position.z;
+ double dx = bodyPos.x - m_pc.pose.position.x;
+ double dy = bodyPos.y - m_pc.pose.position.y;
+ double dz = bodyPos.z - m_pc.pose.position.z;
*distPos = dx * dx + dy * dy + dz * dz;
}
@@ -84,8 +121,8 @@
*distAng = 0.0;
break;
- case robot_msgs::KinematicConstraint::ONLY_ORIENTATION:
- case robot_msgs::KinematicConstraint::COMPLETE_POSE:
+ case robot_msgs::PoseConstraint::ONLY_ORIENTATION:
+ case robot_msgs::PoseConstraint::COMPLETE_POSE:
default:
if (distPos)
*distPos = 0.0;
@@ -103,29 +140,21 @@
}
}
- bool decide(void)
- {
- double dPos, dAng;
- evaluate(&dPos, &dAng);
-
- return decide(dPos, dAng);
- }
-
bool decide(double dPos, double dAng)
{
bool result = true;
- switch (m_kc.type)
+ switch (m_pc.type)
{
- case robot_msgs::KinematicConstraint::ONLY_POSITION:
- result = dPos < m_kc.position_distance;
+ case robot_msgs::PoseConstraint::ONLY_POSITION:
+ result = dPos < m_pc.position_distance;
break;
- case robot_msgs::KinematicConstraint::ONLY_ORIENTATION:
- result = dAng < m_kc.orientation_distance;
+ case robot_msgs::PoseConstraint::ONLY_ORIENTATION:
+ result = dAng < m_pc.orientation_distance;
break;
- case robot_msgs::KinematicConstraint::COMPLETE_POSE:
- result = (dPos < m_kc.position_distance) && (dAng < m_kc.orientation_distance);
+ case robot_msgs::PoseConstraint::COMPLETE_POSE:
+ result = (dPos < m_pc.position_distance) && (dAng < m_pc.orientation_distance);
break;
default:
@@ -134,17 +163,17 @@
return result;
}
- void print(std::ostream &out = std::cout) const
+ virtual void print(std::ostream &out = std::cout) const
{
if (m_link)
{
- out << "Constraint on link '" << m_kc.robot_link << "'" << std::endl;
- if (m_kc.type != robot_msgs::KinematicConstraint::ONLY_ORIENTATION)
- out << " Desired position: " << m_kc.pose.position.x << ", " << m_kc.pose.position.y << ", " << m_kc.pose.position.z
- << " (within distance: " << m_kc.position_distance << ")" << std::endl;
- if (m_kc.type != robot_msgs::KinematicConstraint::ONLY_POSITION)
- out << " Desired orientation: " << m_kc.pose.orientation.x << ", " << m_kc.pose.orientation.y << ", " << m_kc.pose.orientation.z << ", " << m_kc.pose.orientation.w
- << " (within distance: " << m_kc.orientation_distance << ")" << std::endl;
+ out << "Constraint on link '" << m_pc.robot_link << "'" << std::endl;
+ if (m_pc.type != robot_msgs::PoseConstraint::ONLY_ORIENTATION)
+ out << " Desired position: " << m_pc.pose.position.x << ", " << m_pc.pose.position.y << ", " << m_pc.pose.position.z
+ << " (within distance: " << m_pc.position_distance << ")" << std::endl;
+ if (m_pc.type != robot_msgs::PoseConstraint::ONLY_POSITION)
+ out << " Desired orientation: " << m_pc.pose.orientation.x << ", " << m_pc.pose.orientation.y << ", " << m_pc.pose.orientation.z << ", " << m_pc.pose.orientation.w
+ << " (within distance: " << m_pc.orientation_distance << ")" << std::endl;
}
else
out << "No constraint" << std::endl;
@@ -152,7 +181,7 @@
protected:
- robot_msgs::KinematicConstraint m_kc;
+ robot_msgs::PoseConstraint m_pc;
planning_models::KinematicModel::Link *m_link;
};
@@ -178,12 +207,12 @@
m_kce.clear();
}
- void use(planning_models::KinematicModel *kmodel, const std::vector<robot_msgs::KinematicConstraint> &kc)
+ void use(planning_models::KinematicModel *kmodel, const std::vector<robot_msgs::PoseConstraint> &kc)
{
clear();
for (unsigned int i = 0 ; i < kc.size() ; ++i)
{
- KinematicConstraintEvaluator *ev = new KinematicConstraintEvaluator();
+ PoseConstraintEvaluator *ev = new PoseConstraintEvaluator();
ev->use(kmodel, kc[i]);
m_kce.push_back(ev);
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RequestPlan.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RequestPlan.h 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RequestPlan.h 2008-08-25 17:18:32 UTC (rev 3549)
@@ -129,9 +129,9 @@
}
/** Set the kinematic constraints to follow */
- void setupConstraints(KinematicPlannerSetup &psetup, const std::vector<robot_msgs::KinematicConstraint> &cstrs)
+ void setupPoseConstraints(KinematicPlannerSetup &psetup, const std::vector<robot_msgs::PoseConstraint> &cstrs)
{
- static_cast<StateValidityPredicate*>(psetup.si->getStateValidityChecker())->setConstraints(cstrs);
+ static_cast<StateValidityPredicate*>(psetup.si->getStateValidityChecker())->setPoseConstraints(cstrs);
}
/** Compute the actual motion plan */
Modified: pkg/trunk/motion_planning/kinematic_planning/include/RequestPlanLinkPosition.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/RequestPlanLinkPosition.h 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/motion_planning/kinematic_planning/include/RequestPlanLinkPosition.h 2008-08-25 17:18:32 UTC (rev 3549)
@@ -44,21 +44,21 @@
{
public:
- GoalToPosition(ompl::SpaceInformation_t si, XMLModel *model, const std::vector<robot_msgs::KinematicConstraint> &kc) : ompl::SpaceInformationKinematic::GoalRegionKinematic(si)
+ GoalToPosition(ompl::SpaceInformation_t si, XMLModel *model, const std::vector<robot_msgs::PoseConstraint> &pc) : ompl::SpaceInformationKinematic::GoalRegionKinematic(si)
{
m_model = model;
- for (unsigned int i = 0 ; i < kc.size() ; ++i)
+ for (unsigned int i = 0 ; i < pc.size() ; ++i)
{
- KinematicConstraintEvaluator *kce = new KinematicConstraintEvaluator();
- kce->use(m_model->kmodel, kc[i]);
- m_kce.push_back(kce);
+ PoseConstraintEvaluator *pce = new PoseConstraintEvaluator();
+ pce->use(m_model->kmodel, pc[i]);
+ m_pce.push_back(pce);
}
}
virtual ~GoalToPosition(void)
{
- for (unsigned int i = 0 ; i < m_kce.size() ; ++i)
- delete m_kce[i];
+ for (unsigned int i = 0 ; i < m_pce.size() ; ++i)
+ delete m_pce[i];
}
virtual double distanceGoal(ompl::SpaceInformationKinematic::StateKinematic_t state)
@@ -79,42 +79,42 @@
return true;
}
+ virtual void print(std::ostream &out = std::cout) const
+ {
+ ompl::SpaceInformationKinematic::GoalRegionKinematic::print(out);
+ for (unsigned int i = 0 ; i < m_pce.size() ; ++i)
+ m_pce[i]->print(out);
+ }
+
+ protected:
+
double evaluateGoalAux(ompl::SpaceInformationKinematic::StateKinematic_t state, std::vector<bool> *decision)
{
update(state);
if (decision)
- decision->resize(m_kce.size());
+ decision->resize(m_pce.size());
double distance = 0.0;
- for (unsigned int i = 0 ; i < m_kce.size() ; ++i)
+ for (unsigned int i = 0 ; i < m_pce.size() ; ++i)
{
double dPos, dAng;
- m_kce[i]->evaluate(&dPos, &dAng);
+ m_pce[i]->evaluate(&dPos, &dAng);
if (decision)
- (*decision)[i] = m_kce[i]->decide(dPos, dAng);
+ (*decision)[i] = m_pce[i]->decide(dPos, dAng);
distance += dPos + dAng;
}
return distance;
}
- virtual void print(std::ostream &out = std::cout) const
- {
- ompl::SpaceInformationKinematic::GoalRegionKinematic::print(out);
- for (unsigned int i = 0 ; i < m_kce.size() ; ++i)
- m_kce[i]->print(out);
- }
-
- protected:
-
void update(ompl::SpaceInformationKinematic::StateKinematic_t state)
{
m_model->kmodel->computeTransforms(static_cast<const ompl::SpaceInformationKinematic::StateKinematic_t>(state)->values, m_model->groupID);
m_model->collisionSpace->updateRobotModel(m_model->collisionSpaceID);
}
- XMLModel *m_model;
- std::vector<KinematicConstraintEvaluator*> m_kce;
+ XMLModel *m_model;
+ std::vector<PoseConstraintEvaluator*> m_pce;
};
@@ -143,10 +143,10 @@
void setupGoalState(XMLModel *model, KinematicPlannerSetup &psetup, robot_srvs::KinematicPlanLinkPosition::request &req)
{
/* set the goal */
- std::vector<robot_msgs::KinematicConstraint> kc;
- req.get_goal_constraints_vec(kc);
+ std::vector<robot_msgs::PoseConstraint> pc;
+ req.get_goal_constraints_vec(pc);
- GoalToPosition *goal = new GoalToPosition(psetup.si, model, kc);
+ GoalToPosition *goal = new GoalToPosition(psetup.si, model, pc);
psetup.si->setGoal(goal);
}
Modified: pkg/trunk/motion_planning/kinematic_planning/include/StateValidityChecker.h
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/include/StateValidityChecker.h 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/motion_planning/kinematic_planning/include/StateValidityChecker.h 2008-08-25 17:18:32 UTC (rev 3549)
@@ -61,7 +61,7 @@
return valid;
}
- void setConstraints(const std::vector<robot_msgs::KinematicConstraint> &kc)
+ void setPoseConstraints(const std::vector<robot_msgs::PoseConstraint> &kc)
{
m_kce.use(m_model->kmodel, kc);
}
@@ -72,8 +72,8 @@
}
protected:
- XMLModel *m_model;
- KinematicConstraintEvaluatorSet m_kce;
+ XMLModel *m_model;
+ KinematicConstraintEvaluatorSet m_kce;
};
#endif
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-25 17:18:32 UTC (rev 3549)
@@ -136,9 +136,9 @@
/* configure state space and starting state */
setupStateSpaceAndStartState(m, psetup, req.params, req.start_state);
- std::vector<robot_msgs::KinematicConstraint> cstrs;
- req.get_constraints_vec(cstrs);
- setupConstraints(psetup, cstrs);
+ std::vector<robot_msgs::PoseConstraint> cstrs;
+ req.constraints.get_pose_vec(cstrs);
+ setupPoseConstraints(psetup, cstrs);
/* add goal state */
RequestPlanState::setupGoalState(m, psetup, req);
@@ -179,10 +179,10 @@
/* configure state space and starting state */
setupStateSpaceAndStartState(m, psetup, req.params, req.start_state);
- std::vector<robot_msgs::KinematicConstraint> cstrs;
- req.get_constraints_vec(cstrs);
- setupConstraints(psetup, cstrs);
-
+ std::vector<robot_msgs::PoseConstraint> cstrs;
+ req.constraints.get_pose_vec(cstrs);
+ setupPoseConstraints(psetup, cstrs);
+
/* add goal state */
RequestPlanLinkPosition::setupGoalState(m, psetup, req);
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-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-25 17:18:32 UTC (rev 3549)
@@ -186,7 +186,7 @@
// the goal region is basically the position of a set of bodies
req.set_goal_constraints_size(1);
- req.goal_constraints[0].type = robot_msgs::KinematicConstraint::ONLY_POSITION;
+ req.goal_constraints[0].type = robot_msgs::PoseConstraint::ONLY_POSITION;
req.goal_constraints[0].robot_link = "wrist_flex_left";
req.goal_constraints[0].pose.position.x = 0.5;
req.goal_constraints[0].pose.position.y = 0.3;
@@ -194,13 +194,13 @@
req.goal_constraints[0].position_distance = 0.01;
// an example of constraints: do not move the elbow too much
- req.set_constraints_size(1);
- req.constraints[0].type = robot_msgs::KinematicConstraint::ONLY_POSITION;
- req.constraints[0].robot_link = "elbow_flex_left";
- req.constraints[0].pose.position.x = 0.45;
- req.constraints[0].pose.position.y = 0.188;
- req.constraints[0].pose.position.z = 0.74;
- req.constraints[0].position_distance = 0.01;
+ req.constraints.set_pose_size(1);
+ req.constraints.pose[0].type = robot_msgs::PoseConstraint::ONLY_POSITION;
+ req.constraints.pose[0].robot_link = "elbow_flex_left";
+ req.constraints.pose[0].pose.position.x = 0.45;
+ req.constraints.pose[0].pose.position.y = 0.188;
+ req.constraints.pose[0].pose.position.z = 0.74;
+ req.constraints.pose[0].position_distance = 0.01;
req.allowed_time = 3.0;
Deleted: pkg/trunk/robot_msgs/msg/KinematicConstraint.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/KinematicConstraint.msg 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/robot_msgs/msg/KinematicConstraint.msg 2008-08-25 17:18:32 UTC (rev 3549)
@@ -1,21 +0,0 @@
-# This message contains the definition of a motion planning constraint.
-# Since there are multiple types of constraints, the 'type' member is used
-# to identify the different constraints
-
-byte COMPLETE_POSE=0 # 0 = complete pose is considered
-byte ONLY_POSITION=1 # 1 = only pose position is considered
-byte ONLY_ORIENTATION=2 # 2 = only pose orientation is considered
-
-byte type
-
-# The robot link this constraint refers to
-string robot_link
-
-# The desired pose of the robot link
-std_msgs/Pose3DFloat64 pose
-
-# the allowed difference (square of euclidian distance) for position
-float64 position_distance
-
-# the allowed difference (shortest angular distance) for orientation
-float64 orientation_distance
Added: pkg/trunk/robot_msgs/msg/KinematicConstraints.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/KinematicConstraints.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/KinematicConstraints.msg 2008-08-25 17:18:32 UTC (rev 3549)
@@ -0,0 +1,3 @@
+# This message contains a list of motion planning constraints.
+
+PoseConstraint[] pose
Added: pkg/trunk/robot_msgs/msg/PoseConstraint.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/PoseConstraint.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/PoseConstraint.msg 2008-08-25 17:18:32 UTC (rev 3549)
@@ -0,0 +1,21 @@
+# This message contains the definition of a motion planning constraint.
+# Since there are multiple types of constraints, the 'type' member is used
+# to identify the different constraints
+
+byte COMPLETE_POSE=0 # 0 = complete pose is considered
+byte ONLY_POSITION=1 # 1 = only pose position is considered
+byte ONLY_ORIENTATION=2 # 2 = only pose orientation is considered
+
+byte type
+
+# The robot link this constraint refers to
+string robot_link
+
+# The desired pose of the robot link
+std_msgs/Pose3DFloat64 pose
+
+# the allowed difference (square of euclidian distance) for position
+float64 position_distance
+
+# the allowed difference (shortest angular distance) for orientation
+float64 orientation_distance
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanLinkPosition.srv 2008-08-25 17:18:32 UTC (rev 3549)
@@ -17,11 +17,11 @@
# The goal state for the model to plan for. The state is not explicit.
# The goal is considered achieved if all the constraints are satisfied.
-robot_msgs/KinematicConstraint[] goal_constraints
+robot_msgs/PoseConstraint[] goal_constraints
# No state in the produced motion plan will violate these constraints
-robot_msgs/KinematicConstraint[] constraints
+robot_msgs/KinematicConstraints constraints
# The number of times this plan is to be computed. Shortest solution
Modified: pkg/trunk/robot_srvs/srv/KinematicPlanState.srv
===================================================================
--- pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2008-08-25 16:38:31 UTC (rev 3548)
+++ pkg/trunk/robot_srvs/srv/KinematicPlanState.srv 2008-08-25 17:18:32 UTC (rev 3549)
@@ -21,7 +21,7 @@
robot_msgs/KinematicState goal_state
# No state in the produced motion plan will violate these constraints
-robot_msgs/KinematicConstraint[] constraints
+robot_msgs/KinematicConstraints constraints
# The number of times this plan is to be computed. Shortest solution
# will be reported.
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-25 17:38:34
|
Revision: 3553
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3553&view=rev
Author: isucan
Date: 2008-08-25 17:38:37 +0000 (Mon, 25 Aug 2008)
Log Message:
-----------
renamed group flag 'plan' to 'planning', added example group for self collision
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/motion_planning/planning_models/src/planning_models/kinematic.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-25 17:32:56 UTC (rev 3552)
+++ pkg/trunk/motion_planning/kinematic_planning/src/kinematic_planning.cpp 2008-08-25 17:38:37 UTC (rev 3553)
@@ -291,7 +291,7 @@
if (group)
{
const robot_desc::URDF::Map &data = group->data;
- std::map<std::string, std::string> info = data.getMapTagValues("plan", "RRT");
+ std::map<std::string, std::string> info = data.getMapTagValues("planning", "RRT");
if (info.find("range") != info.end())
{
@@ -334,7 +334,7 @@
if (group)
{
const robot_desc::URDF::Map &data = group->data;
- std::map<std::string, std::string> info = data.getMapTagValues("plan", "LazyRRT");
+ std::map<std::string, std::string> info = data.getMapTagValues("planning", "LazyRRT");
if (info.find("range") != info.end())
{
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-08-25 17:32:56 UTC (rev 3552)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-25 17:38:37 UTC (rev 3553)
@@ -101,7 +101,7 @@
{
robot_srvs::KinematicPlanState::request req;
- req.params.model_id = "pr2::leftArm";
+ req.params.model_id = "pr2::left_arm";
req.params.distance_metric = "L2Square";
req.threshold = 0.01;
req.interpolate = 1;
@@ -127,7 +127,7 @@
{
robot_srvs::KinematicPlanState::request req;
- req.params.model_id = "pr2::rightArm";
+ req.params.model_id = "pr2::right_arm";
req.params.distance_metric = "L2Square";
req.threshold = 0.01;
req.interpolate = 1;
@@ -176,7 +176,7 @@
{
robot_srvs::KinematicPlanLinkPosition::request req;
- req.params.model_id = "pr2::leftArm";
+ req.params.model_id = "pr2::left_arm";
req.params.distance_metric = "L2Square";
req.params.planner_id = "RRT";
req.interpolate = 1;
Modified: pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp 2008-08-25 17:32:56 UTC (rev 3552)
+++ pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp 2008-08-25 17:38:37 UTC (rev 3553)
@@ -247,7 +247,7 @@
model.getGroupNames(allGroups);
m_groups.clear();
for (unsigned int i = 0 ; i < allGroups.size() ; ++i)
- if (model.getGroup(allGroups[i])->hasFlag("plan"))
+ if (model.getGroup(allGroups[i])->hasFlag("planning"))
m_groups.push_back(rname + "::" + allGroups[i]);
m_groupsMap.clear();
for (unsigned int i = 0 ; i < m_groups.size() ; ++i)
@@ -362,7 +362,7 @@
std::vector<std::string> gnames;
model.getGroupNames(gnames);
for (unsigned int i = 0 ; i < gnames.size() ; ++i)
- if (model.getGroup(gnames[i])->hasFlag("plan"))
+ if (model.getGroup(gnames[i])->hasFlag("planning"))
joint->inGroup.push_back(urdfLink->inGroup[i]);
for (unsigned int i = 0 ; i < joint->inGroup.size() ; ++i)
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-25 17:32:56 UTC (rev 3552)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-25 17:38:37 UTC (rev 3553)
@@ -4,7 +4,7 @@
<!-- Define the parts of the robot that the robot's scanners can see -->
- <group name="self_see">
+ <group name="self_see" flags="mapping">
upperarm_roll_left
upperarm_roll_right
elbow_flex_left
@@ -21,16 +21,21 @@
finger_r_right
</group>
+ <!-- Define groups to check for self collision -->
+
+ <group name="left_arm" flags="self_collision">
+ elbow_flex_left
+ base
+ </group>
-
- <!-- Groups needed for motion planning (flag contains "plan")
+ <!-- Groups needed for motion planning (flag contains "planning")
and for kinematics (flag contains "kinematic") -->
- <group name="base" flags="plan">
+ <group name="base" flags="planning">
base
</group>
- <group name="leftArm" flags="plan kinematic">
+ <group name="left_arm" flags="planning kinematic">
shoulder_pan_left
shoulder_pitch_left
upperarm_roll_left
@@ -39,16 +44,16 @@
wrist_flex_left
gripper_roll_left
- <map name="RRT" flag="plan">
+ <map name="RRT" flag="planning">
<elem key="range" value="0.75" />
</map>
- <map name="LazyRRT" flag="plan">
+ <map name="LazyRRT" flag="planning">
<elem key="range" value="0.75" />
</map>
</group>
- <group name="rightArm" flags="plan kinematic">
+ <group name="right_arm" flags="planning kinematic">
shoulder_pan_right
shoulder_pitch_right
@@ -58,16 +63,16 @@
wrist_flex_right
gripper_roll_right
- <map name="RRT" flag="plan">
+ <map name="RRT" flag="planning">
<elem key="range" value="0.75" />
</map>
- <map name="LazyRRT" flag="plan">
+ <map name="LazyRRT" flag="planning">
<elem key="range" value="0.75" />
</map>
</group>
- <group name="base+leftArm" flags="plan">
+ <group name="base_left_arm" flags="planning">
base
torso
shoulder_pan_left
@@ -79,7 +84,7 @@
gripper_roll_left
</group>
- <group name="base+rightArm" flags="plan">
+ <group name="base_right_arm" flags="planning">
base
torso
shoulder_pan_right
@@ -91,7 +96,7 @@
gripper_roll_right
</group>
- <group name="arms" flags="plan">
+ <group name="arms" flags="planning">
shoulder_pan_left
shoulder_pitch_left
upperarm_roll_left
@@ -108,7 +113,7 @@
gripper_roll_right
</group>
- <group name="base+arms" flags="plan">
+ <group name="base_arms" flags="planning">
base
torso
shoulder_pan_left
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-25 17:32:56 UTC (rev 3552)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-25 17:38:37 UTC (rev 3553)
@@ -271,7 +271,7 @@
void addSelfSeeBodies(void)
{
robot_desc::URDF::Group *ss = m_urdf->getGroup("self_see");
- if (ss)
+ if (ss && ss->hasFlag("mapping"))
{
for (unsigned int i = 0 ; i < ss->linkNames.size() ; ++i)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-08-25 19:15:32
|
Revision: 3560
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3560&view=rev
Author: gerkey
Date: 2008-08-25 19:15:39 +0000 (Mon, 25 Aug 2008)
Log Message:
-----------
adding more svn:ignore properties
Property Changed:
----------------
pkg/trunk/robot_srvs/
pkg/trunk/util/libTF/
pkg/trunk/util/libTF/test/
pkg/trunk/util/logsetta/
pkg/trunk/util/mux/
pkg/trunk/util/random_utils/
Property changes on: pkg/trunk/robot_srvs
___________________________________________________________________
Added: svn:ignore
+ src
Property changes on: pkg/trunk/util/libTF
___________________________________________________________________
Modified: svn:ignore
- simpletest
pose3d_unittest
test_interp
testMatrix
testtf
testMatrixQuaternion
+ simpletest
pose3d_unittest
test_interp
testMatrix
testtf
testMatrixQuaternion
libTF_unittest
Property changes on: pkg/trunk/util/libTF/test
___________________________________________________________________
Added: svn:ignore
+ pose3d_unittest
Property changes on: pkg/trunk/util/logsetta
___________________________________________________________________
Added: svn:ignore
+ bin
Property changes on: pkg/trunk/util/mux
___________________________________________________________________
Added: svn:ignore
+ mux
switch
Property changes on: pkg/trunk/util/random_utils
___________________________________________________________________
Added: svn:ignore
+ test_random_utils
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-08-25 19:20:17
|
Revision: 3561
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3561&view=rev
Author: hsujohnhsu
Date: 2008-08-25 19:20:21 +0000 (Mon, 25 Aug 2008)
Log Message:
-----------
update laser plugin to read in sigma for artificial noise via xml tags in gazebo <gaussianNoise>.
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2_test.xml
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-08-25 19:15:39 UTC (rev 3560)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-08-25 19:20:21 UTC (rev 3561)
@@ -121,6 +121,8 @@
// FIXME: extract link name directly?
private: std::string frameName;
+ private: double gaussianNoise;
+
private: double GaussianKernel(double mu,double sigma);
// A mutex to lock access to fields that are used in message callbacks
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc 2008-08-25 19:15:39 UTC (rev 3560)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc 2008-08-25 19:20:21 UTC (rev 3561)
@@ -98,6 +98,7 @@
std::cout << "================= " << this->topicName << std::endl;
rosnode->advertise<std_msgs::LaserScan>(this->topicName);
this->frameName = node->GetString("frameName","default_ros_laser",0); //read from xml file
+ this->gaussianNoise = node->GetDouble("gaussianNoise",0.0,0); //read from xml file
}
////////////////////////////////////////////////////////////////////////////////
@@ -221,12 +222,11 @@
/* point scan from laser */
/* */
/***************************************************************/
- double sigma = 0.002; // 2 milimeter noise
if (r == maxRange)
this->laserMsg.ranges[i] = r; // no noise if at max range
else
- this->laserMsg.ranges[i] = r + this->GaussianKernel(0,sigma) ;
- this->laserMsg.intensities[i] = v + this->GaussianKernel(0,sigma) ;
+ this->laserMsg.ranges[i] = r + this->GaussianKernel(0,this->gaussianNoise) ;
+ this->laserMsg.intensities[i] = v + this->GaussianKernel(0,this->gaussianNoise) ;
}
// iface writing can be skipped if iface is not used.
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-08-25 19:15:39 UTC (rev 3560)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-08-25 19:20:21 UTC (rev 3561)
@@ -1675,6 +1675,7 @@
<map name="sensor" flag="gazebo">
<verbatim key="sensor_ray">
<sensor:ray name="tilt_laser">
+ <gaussianNoise>0.05</gaussianNoise>
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1736,6 +1737,7 @@
<map name="sensor" flag="gazebo">
<verbatim key="sensor_ray">
<sensor:ray name="base_laser">
+ <gaussianNoise>0.05</gaussianNoise>
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2_test.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2_test.xml 2008-08-25 19:15:39 UTC (rev 3560)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2_test.xml 2008-08-25 19:20:21 UTC (rev 3561)
@@ -1672,6 +1672,7 @@
<map name="sensor" flag="gazebo">
<verbatim key="sensor_ray">
<sensor:ray name="tilt_laser">
+ <gaussianNoise>0.05</gaussianNoise>
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
@@ -1733,6 +1734,7 @@
<map name="sensor" flag="gazebo">
<verbatim key="sensor_ray">
<sensor:ray name="base_laser">
+ <gaussianNoise>0.05</gaussianNoise>
<rayCount>683</rayCount>
<rangeCount>683</rangeCount>
<laserCount>1</laserCount>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-08-25 19:34:02
|
Revision: 3563
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3563&view=rev
Author: gerkey
Date: 2008-08-25 19:34:08 +0000 (Mon, 25 Aug 2008)
Log Message:
-----------
added more svn:ignores
Property Changed:
----------------
pkg/trunk/pr2_srvs/
pkg/trunk/visualization/wx_roserr/bin/
Property changes on: pkg/trunk/pr2_srvs
___________________________________________________________________
Added: svn:ignore
+ src
Property changes on: pkg/trunk/visualization/wx_roserr/bin
___________________________________________________________________
Added: svn:ignore
+ roserr_send_test
roserr_test
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-25 21:34:53
|
Revision: 3576
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3576&view=rev
Author: isucan
Date: 2008-08-25 21:34:51 +0000 (Mon, 25 Aug 2008)
Log Message:
-----------
added self collision checking
Modified Paths:
--------------
pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h
pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h
pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h
pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h 2008-08-25 21:34:51 UTC (rev 3576)
@@ -40,6 +40,7 @@
#include <planning_models/kinematic.h>
#include <rosthread/mutex.h>
#include <vector>
+#include <string>
/** @htmlinclude ../../manifest.html
@@ -78,10 +79,13 @@
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id) = 0;
+ /** Add a group of links to be checked for self collision */
+ virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links) = 0;
+
/** Get the number of loaded models */
unsigned int getModelCount(void) const;
/** Get a specific model */
- planning_models::KinematicModel* getModel(unsigned int model_id) const;
+ planning_models::KinematicModel* getRobotModel(unsigned int model_id) const;
/** Provide interface to a lock. Use carefully! */
void lock(void);
@@ -93,8 +97,8 @@
void setSelfCollision(bool selfCollision);
/** Check if self collision is enabled */
- bool getSelfCollision(void) const;
-
+ bool getSelfCollision(void) const;
+
protected:
ros::thread::mutex m_lock;
@@ -103,6 +107,8 @@
/** List of loaded robot models */
std::vector<planning_models::KinematicModel*> m_models;
+
+
};
}
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-08-25 21:34:51 UTC (rev 3576)
@@ -87,6 +87,9 @@
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id);
+ /** Add a group of links to be checked for self collision */
+ virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links);
+
protected:
class ODECollide2
@@ -161,8 +164,9 @@
struct ModelInfo
{
- std::vector< kGeom* > g;
- dSpaceID s;
+ std::vector< kGeom* > geom;
+ dSpaceID space;
+ std::vector< std::vector<unsigned int> > selfCollision;
};
dGeomID createODEGeom(dSpaceID space, planning_models::KinematicModel::Shape *shape) const;
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-08-25 21:34:51 UTC (rev 3576)
@@ -59,13 +59,13 @@
m_octree(0.0f, 0.0f, 0.0f, 0.02f, 0.02f, 0.02f, 1, OCTREE_CELL_EMPTY)
{
m_octree.setAutoExpand(true);
+ m_selfCollision = false;
}
virtual ~EnvironmentModelOctree(void)
{
}
-
/** Check if a model is in collision */
virtual bool isCollision(unsigned int model_id);
@@ -81,6 +81,9 @@
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id);
+ /** Add a group of links to be checked for self collision */
+ virtual void addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links);
+
const scan_utils::Octree<char>* getOctree(void) const;
protected:
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp 2008-08-25 21:34:51 UTC (rev 3576)
@@ -69,7 +69,7 @@
return m_models.size();
}
-planning_models::KinematicModel* collision_space::EnvironmentModel::getModel(unsigned int model_id) const
+planning_models::KinematicModel* collision_space::EnvironmentModel::getRobotModel(unsigned int model_id) const
{
return m_models[model_id];
}
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-08-25 21:34:51 UTC (rev 3576)
@@ -40,9 +40,9 @@
{
for (unsigned int i = 0 ; i < m_kgeoms.size() ; ++i)
{
- for (unsigned int j = 0 ; j < m_kgeoms[i].g.size() ; ++j)
- delete m_kgeoms[i].g[j];
- dSpaceDestroy(m_kgeoms[i].s);
+ for (unsigned int j = 0 ; j < m_kgeoms[i].geom.size() ; ++j)
+ delete m_kgeoms[i].geom[j];
+ dSpaceDestroy(m_kgeoms[i].space);
}
if (m_space)
dSpaceDestroy(m_space);
@@ -55,7 +55,7 @@
if (m_kgeoms.size() <= id)
{
m_kgeoms.resize(id + 1);
- m_kgeoms[id].s = dHashSpaceCreate(0);
+ m_kgeoms[id].space = dHashSpaceCreate(0);
}
for (unsigned int j = 0 ; j < m_models[id]->getRobotCount() ; ++j)
@@ -65,11 +65,11 @@
{
kGeom *kg = new kGeom();
kg->link = robot->links[i];
- dGeomID g = createODEGeom(m_kgeoms[id].s, robot->links[i]->shape);
+ dGeomID g = createODEGeom(m_kgeoms[id].space, robot->links[i]->shape);
if (g)
{
kg->geom = g;
- m_kgeoms[id].g.push_back(kg);
+ m_kgeoms[id].geom.push_back(kg);
}
else
delete kg;
@@ -108,12 +108,12 @@
void collision_space::EnvironmentModelODE::updateRobotModel(unsigned int model_id)
{
- const unsigned int n = m_kgeoms[model_id].g.size();
+ const unsigned int n = m_kgeoms[model_id].geom.size();
for (unsigned int i = 0 ; i < n ; ++i)
{
- libTF::Pose3D &pose = m_kgeoms[model_id].g[i]->link->globalTrans;
- dGeomID geom = m_kgeoms[model_id].g[i]->geom;
+ libTF::Pose3D &pose = m_kgeoms[model_id].geom[i]->link->globalTrans;
+ dGeomID geom = m_kgeoms[model_id].geom[i]->geom;
libTF::Pose3D::Position pos = pose.getPosition();
dGeomSetPosition(geom, pos.x, pos.y, pos.z);
@@ -190,7 +190,7 @@
dSpaceID collision_space::EnvironmentModelODE::getModelODESpace(unsigned int model_id) const
{
- return m_kgeoms[model_id].s;
+ return m_kgeoms[model_id].space;
}
struct CollisionData
@@ -216,12 +216,48 @@
{
CollisionData cdata;
cdata.collides = false;
- m_collide2.setup();
- for (int i = m_kgeoms[model_id].g.size() - 1 ; i >= 0 && !cdata.collides ; --i)
- m_collide2.collide(m_kgeoms[model_id].g[i]->geom, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+
+ if (m_selfCollision)
+ {
+ for (int i = m_kgeoms[model_id].selfCollision.size() - 1 ; i >= 0 ; --i)
+ {
+ const std::vector<unsigned int> &vec = m_kgeoms[model_id].selfCollision[i];
+ unsigned int n = vec.size();
+
+ for (unsigned int j = 0 ; j < n ; ++j)
+ for (unsigned int k = j + 1 ; k < n ; ++k)
+ {
+ // dSpaceCollide2 expects AABBs to be computed, so
+ // we force that by calling dGeomGetAABB. Since we
+ // get the data anyway, we attempt to speed things
+ // up using it.
+ dGeomID g1 = m_kgeoms[model_id].geom[vec[j]]->geom;
+ dGeomID g2 = m_kgeoms[model_id].geom[vec[k]]->geom;
+ dReal aabb1[6], aabb2[6];
+ dGeomGetAABB(g1, aabb1);
+ dGeomGetAABB(g2, aabb2);
+ if (!(aabb1[2] > aabb2[3] ||
+ aabb1[3] < aabb2[2] ||
+ aabb1[4] > aabb2[5] ||
+ aabb1[5] < aabb2[4]))
+ dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ if (cdata.collides)
+ {
+ std::cout << m_kgeoms[model_id].geom[vec[j]]->link->name << " intersects with " << m_kgeoms[model_id].geom[vec[k]]->link->name << std::endl;
+ goto OUT;
+ }
+ }
+ }
+ }
- if (m_selfCollision && !cdata.collides)
- dSpaceCollide(m_kgeoms[model_id].s, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ OUT:
+
+ if (!cdata.collides)
+ {
+ m_collide2.setup();
+ for (int i = m_kgeoms[model_id].geom.size() - 1 ; i >= 0 && !cdata.collides ; --i)
+ m_collide2.collide(m_kgeoms[model_id].geom[i]->geom, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ }
return cdata.collides;
}
@@ -246,3 +282,18 @@
m_space = dHashSpaceCreate(0);
m_collide2.setup();
}
+
+void collision_space::EnvironmentModelODE::addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links)
+{
+ if (model_id < m_kgeoms.size())
+ {
+ unsigned int pos = m_kgeoms[model_id].selfCollision.size();
+ m_kgeoms[model_id].selfCollision.resize(pos + 1);
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ {
+ for (unsigned int j = 0 ; j < m_kgeoms[model_id].geom.size() ; ++j)
+ if (m_kgeoms[model_id].geom[j]->link->name == links[i])
+ m_kgeoms[model_id].selfCollision[pos].push_back(j);
+ }
+ }
+}
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-08-25 21:34:51 UTC (rev 3576)
@@ -137,3 +137,8 @@
{
return &m_octree;
}
+
+void collision_space::EnvironmentModelOctree::addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links)
+{
+ fprintf(stderr, "Octree collision checking does not support self collision\n");
+}
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-25 21:34:51 UTC (rev 3576)
@@ -93,7 +93,7 @@
m_collisionSpace = collisionSpace;
else
m_collisionSpace = new collision_space::EnvironmentModelODE();
- m_collisionSpace->setSelfCollision(false);
+ m_collisionSpace->setSelfCollision(true);
m_sphereSize = 0.03;
@@ -115,8 +115,9 @@
if (m_kmodel)
{
m_collisionSpace->lock();
- m_collisionSpace->addRobotModel(m_kmodel);
+ unsigned int cid = m_collisionSpace->addRobotModel(m_kmodel);
m_collisionSpace->unlock();
+ addSelfCollisionGroups(cid, file);
}
}
@@ -133,6 +134,18 @@
collision_space::EnvironmentModel *m_collisionSpace;
double m_sphereSize;
+ void addSelfCollisionGroups(unsigned int cid, robot_desc::URDF *model)
+ {
+ std::vector<robot_desc::URDF::Group*> groups;
+ model->getGroups(groups);
+
+ m_collisionSpace->lock();
+ for (unsigned int i = 0 ; i < groups.size() ; ++i)
+ if (groups[i]->hasFlag("self_collision"))
+ m_collisionSpace->addSelfCollisionGroup(cid, groups[i]->linkNames);
+ m_collisionSpace->unlock();
+ }
+
void worldMapCallback(void)
{
unsigned int n = m_worldCloud.get_pts_size();
Modified: pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-25 21:34:51 UTC (rev 3576)
@@ -157,9 +157,9 @@
if (m_collisionSpace && m_collisionSpace->getModelCount() == 1 && m_follow)
{
- int group = m_collisionSpace->getModel(0)->getGroupID("pr2::base");
+ int group = m_collisionSpace->getRobotModel(0)->getGroupID("pr2::base");
m_collisionSpace->lock();
- m_collisionSpace->getModel(0)->computeTransforms(m_basePos, group);
+ m_collisionSpace->getRobotModel(0)->computeTransforms(m_basePos, group);
m_collisionSpace->updateRobotModel(0);
m_collisionSpace->unlock();
checkCollision();
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-25 21:29:05 UTC (rev 3575)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-25 21:34:51 UTC (rev 3576)
@@ -22,8 +22,8 @@
</group>
<!-- Define groups to check for self collision -->
-
- <group name="left_arm" flags="self_collision">
+
+ <group name="collisions1" flags="self_collision">
elbow_flex_left
base
</group>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-08-26 02:26:20
|
Revision: 3601
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3601&view=rev
Author: hsujohnhsu
Date: 2008-08-26 02:26:26 +0000 (Tue, 26 Aug 2008)
Log Message:
-----------
* update gazebo to new revision (major patch rework).
* update opende to new revision (no major changes).
* udpate camera and laser plugins corresponding to new gazebo revision.
* updates to controller effort limits for pr2_test.xml.
* removed torso joint from arm test case.
* added services from Advait's grasp utilities. Though not yet implemented in the arm controller or anywhere else.
* update to gazebo world files for <grid> tag, now use "false" instead of "off".
* 3rdparty/gazebo/patches/camera_saveframe_tony.diff is new, but not yet implemented into gazebo, so camera test will not work yet.
* removed map-elem tags from controllers.xml, they are now in gazebo_joints.xml because they are gazebo specific.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/Makefile
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/3rdparty/gazebo/patches/camera_saveframe_tony.diff
pkg/trunk/3rdparty/opende/Makefile
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
pkg/trunk/drivers/simulator/gazebo_plugin/CMakeLists.txt
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Camera.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/Ros_Laser.cc
pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_arm_test.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/pr2_test.xml
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_floorobj.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_obs.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_rosgazebo.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_rosgazebo_wg.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_tablegrasp.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test.world
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/gazebo_joints.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2_test.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/gazebo_joints_arm_test.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/pr2_arm_test.xml
Added Paths:
-----------
pkg/trunk/drivers/simulator/gazebo_plugin/msg/
pkg/trunk/drivers/simulator/gazebo_plugin/msg/CartesianCmd.msg
pkg/trunk/drivers/simulator/gazebo_plugin/srv/
pkg/trunk/drivers/simulator/gazebo_plugin/srv/GripperCmd.srv
pkg/trunk/drivers/simulator/gazebo_plugin/srv/MoveCartesian.srv
pkg/trunk/drivers/simulator/gazebo_plugin/srv/VoidVoid.srv
Modified: pkg/trunk/3rdparty/gazebo/Makefile
===================================================================
--- pkg/trunk/3rdparty/gazebo/Makefile 2008-08-26 02:11:58 UTC (rev 3600)
+++ pkg/trunk/3rdparty/gazebo/Makefile 2008-08-26 02:26:26 UTC (rev 3601)
@@ -2,7 +2,7 @@
SVN_DIR = gazebo-svn
SVN_URL = https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/gazebo/trunk
-SVN_REVISION = -r 6952 #more updates on geometry creation
+SVN_REVISION = -r 6981
SVN_PATCH = gazebo_patch.diff
include $(shell rospack find mk)/svn_checkout.mk
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-08-26 02:11:58 UTC (rev 3600)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-08-26 02:26:26 UTC (rev 3601)
@@ -1,6 +1,6 @@
Index: player/SConscript
===================================================================
---- player/SConscript (revision 6952)
+--- player/SConscript (revision 6981)
+++ player/SConscript (working copy)
@@ -1,7 +1,8 @@
import os
@@ -12,98 +12,9 @@
sources = ['GazeboDriver.cc',
'GazeboClient.cc',
-Index: libgazebo/Server.cc
-===================================================================
---- libgazebo/Server.cc (revision 6952)
-+++ libgazebo/Server.cc (working copy)
-@@ -38,6 +38,7 @@
- #include <sys/sem.h>
- #include <sstream>
- #include <iostream>
-+#include <signal.h>
-
- #include "gazebo.h"
-
-@@ -92,6 +93,42 @@
-
- std::cout << "creating " << this->filename << "\n";
-
-+ // check to see if there is already a directory created.
-+ struct stat astat;
-+ if (stat(this->filename.c_str(), &astat) == 0) {
-+ // directory already exists, check gazebo.pid to see if
-+ // another gazebo is already running.
-+
-+ std::string pidfn = this->filename + "/gazebo.pid";
-+
-+ FILE *fp = fopen(pidfn.c_str(), "r");
-+ if(fp) {
-+ int pid;
-+ fscanf(fp, "%d", &pid);
-+ fclose(fp);
-+ std::cout << "found a pid file: pid=" << pid << "\n";
-+
-+ if(kill(pid, 0) == 0) {
-+ // a gazebo process is still alive.
-+ errStream << "directory [" << this->filename
-+ << "] already exists (previous crash?)\n"
-+ << "gazebo (pid=" << pid << ") is still running.";
-+ throw(errStream.str());
-+ } else {
-+ // the gazebo process is not alive.
-+ // remove directory.
-+ std::cout << "The gazebo process is not alive.\n";
-+
-+ // remove the existing directory.
-+ std::string cmd = "rm -rf '" + this->filename + "'";
-+ if(system(cmd.c_str()) != 0) {
-+ errStream << "couldn't remove directory [" << this->filename << "]";
-+ throw(errStream.str());
-+ }
-+ }
-+ }
-+ }
-+
- // Create the directory
- if (mkdir(this->filename.c_str(), S_IRUSR | S_IWUSR | S_IXUSR) != 0)
- {
-@@ -108,7 +145,17 @@
- << strerror(errno) << "]";
- throw(errStream.str());
- }
-+
- }
-+
-+ // write the PID to a file
-+ std::string pidfn = this->filename + "/gazebo.pid";
-+
-+ FILE *fp = fopen(pidfn.c_str(), "w");
-+ if(fp) {
-+ fprintf(fp, "%d\n", getpid());
-+ fclose(fp);
-+ }
- }
-
-
-@@ -120,6 +167,15 @@
-
- std::cout << "deleting " << this->filename << "\n";
-
-+ // unlink the pid file
-+ std::string pidfn = this->filename + "/gazebo.pid";
-+ if (unlink(pidfn.c_str()) < 0)
-+ {
-+ std::ostringstream stream;
-+ stream << "error deleting pid file: " << strerror(errno);
-+ throw(stream.str());
-+ }
-+
- // Delete the server dir
- if (rmdir(this->filename.c_str()) != 0)
- {
Index: libgazebo/Iface.cc
===================================================================
---- libgazebo/Iface.cc (revision 6952)
+--- libgazebo/Iface.cc (revision 6981)
+++ libgazebo/Iface.cc (working copy)
@@ -55,6 +55,8 @@
GZ_REGISTER_IFACE("factory", FactoryIface);
@@ -116,7 +27,7 @@
Index: libgazebo/gazebo.h
===================================================================
---- libgazebo/gazebo.h (revision 6952)
+--- libgazebo/gazebo.h (revision 6981)
+++ libgazebo/gazebo.h (working copy)
@@ -558,7 +558,7 @@
@@ -605,132 +516,204 @@
};
Index: server/physics/SphereGeom.cc
===================================================================
---- server/physics/SphereGeom.cc (revision 6952)
+--- server/physics/SphereGeom.cc (revision 6981)
+++ server/physics/SphereGeom.cc (working copy)
-@@ -53,11 +53,15 @@
- double radius = node->GetDouble("size",1.0,0);
+@@ -55,10 +55,18 @@
+ this->radiusP->Load(node);
// Initialize box mass matrix
-- dMassSetSphereTotal(&this->mass, this->dblMass, radius);
-+ this->SetGeom( dCreateSphere(0, radius ), true);
+- dMassSetSphereTotal(&this->mass, this->massP->GetValue(), this->radiusP->GetValue());
++ this->SetGeom( dCreateSphere(0, this->radiusP->GetValue()), true);
// Create the sphere geometry
-- this->SetGeom( dCreateSphere(0, radius ), true);
--
-+ if (this->massMatrix)
-+ dMassSetParameters(&this->mass, this->dblMass, this->cx, this->cy, this->cz,
-+ this->ixx,this->iyy,this->izz,this->ixy,this->ixz,this->iyz);
+- this->SetGeom( dCreateSphere(0, this->radiusP->GetValue()), true);
++ if (this->customMassMatrix)
++ dMassSetParameters(&this->mass, this->massP->GetValue(),
++ this->cx, this->cy, this->cz,
++ this->ixx,this->iyy,this->izz,
++ this->ixy,this->ixz,this->iyz);
+ else
-+ dMassSetSphereTotal(&this->mass, this->dblMass, radius);
++ dMassSetSphereTotal(&this->mass, this->massP->GetValue(), this->radiusP->GetValue());
++
+
- //to be able to show physics
- /* this->visualNode->AttachMesh("unit_sphere"); // unit_sphere radius=1 diameter=2
- this->visualNode->SetScale(Vector3(radius, radius ,radius));
+ }
+
+ //////////////////////////////////////////////////////////////////////////////
Index: server/physics/BoxGeom.cc
===================================================================
---- server/physics/BoxGeom.cc (revision 6952)
+--- server/physics/BoxGeom.cc (revision 6981)
+++ server/physics/BoxGeom.cc (working copy)
-@@ -54,11 +54,18 @@
+@@ -54,16 +54,25 @@
+ {
+ this->sizeP->Load(node);
- size = node->GetVector3("size",Vector3(1,1,1));
-
-+ // Create a box geometry with box mass matrix
-+ this->SetGeom(dCreateBox( 0, size.x, size.y, size.z), true );
+- // Initialize box mass matrix
+- dMassSetBoxTotal(&this->mass, this->massP->GetValue(),
+- this->sizeP->GetValue().x,
+- this->sizeP->GetValue().y,
+- this->sizeP->GetValue().z);
+-
+ // Create a box geometry with box mass matrix
+ this->SetGeom(dCreateBox( 0, this->sizeP->GetValue().x,
+ this->sizeP->GetValue().y,
+ this->sizeP->GetValue().z), true );
+
- // Initialize box mass matrix
-- dMassSetBoxTotal(&this->mass, this->dblMass, size.x, size.y, size.z);
++ // Initialize box mass matrix
+ // set mass matrix if user provides some info
+ // pending a tag <massMatrix>true</massMatrix> in geom:
-+ if (this->massMatrix)
-+ dMassSetParameters(&this->mass, this->dblMass, this->cx, this->cy, this->cz,
-+ this->ixx,this->iyy,this->izz,this->ixy,this->ixz,this->iyz);
++ if (this->customMassMatrix)
++ dMassSetParameters(&this->mass, this->massP->GetValue(),
++ this->cx, this->cy, this->cz,
++ this->ixx,this->iyy,this->izz,
++ this->ixy,this->ixz,this->iyz);
+ else
-+ dMassSetBoxTotal(&this->mass, this->dblMass, size.x, size.y, size.z);
++ dMassSetBoxTotal(&this->mass, this->massP->GetValue(),
++ this->sizeP->GetValue().x,
++ this->sizeP->GetValue().y,
++ this->sizeP->GetValue().z);
++
+ }
-- // Create a box geometry with box mass matrix
-- this->SetGeom(dCreateBox( 0, size.x, size.y, size.z), true );
- /* this->visualNode->AttachMesh("unit_box");
- this->visualNode->SetScale(size);
- this->visualNode->SetMaterial("Gazebo/GreenEmissive");
+ //////////////////////////////////////////////////////////////////////////////
Index: server/physics/Geom.hh
===================================================================
---- server/physics/Geom.hh (revision 6952)
+--- server/physics/Geom.hh (revision 6981)
+++ server/physics/Geom.hh (working copy)
-@@ -164,6 +164,10 @@
+@@ -168,6 +168,20 @@
/// Mass as a double
- protected: double dblMass;
+ protected: Param<double> *massP;
+ /// User specified Mass Matrix
-+ protected: bool massMatrix;
++ protected: Param<bool> *customMassMatrixP;
++ protected: Param<double> *cxP ;
++ protected: Param<double> *cyP ;
++ protected: Param<double> *czP ;
++ protected: Param<double> *ixxP;
++ protected: Param<double> *iyyP;
++ protected: Param<double> *izzP;
++ protected: Param<double> *ixyP;
++ protected: Param<double> *ixzP;
++ protected: Param<double> *iyzP;
++ protected: bool customMassMatrix;
+ protected: double cx,cy,cz,ixx,iyy,izz,ixy,ixz,iyz;
+
- /// Special bounding box visual
- private: OgreVisual *bbVisual;
+ private: Param<Vector3> *xyzP;
+ private: Param<Quatern> *rpyP;
Index: server/physics/HingeJoint.hh
===================================================================
---- server/physics/HingeJoint.hh (revision 6952)
+--- server/physics/HingeJoint.hh (revision 6981)
+++ server/physics/HingeJoint.hh (working copy)
-@@ -117,6 +117,8 @@
-
- /// Set the torque of a joint.
- public: void SetTorque(double torque);
-+
-+ public: Vector3 anchorOffset;
- };
- /// \}
-
+@@ -126,6 +126,7 @@
+ private: Param<Vector3> *axisP;
+ private: Param<Angle> *loStopP;
+ private: Param<Angle> *hiStopP;
++ private: Param<Vector3> *anchorOffsetP;
+ };
+ /// \}
+
Index: server/physics/CylinderGeom.cc
===================================================================
---- server/physics/CylinderGeom.cc (revision 6952)
+--- server/physics/CylinderGeom.cc (revision 6981)
+++ server/physics/CylinderGeom.cc (working copy)
-@@ -50,10 +50,17 @@
- double radius = node->GetTupleDouble("size",0,1.0);
- double length = node->GetTupleDouble("size",1,1.0);
+@@ -51,13 +51,22 @@
+ {
+ this->sizeP->Load(node);
+- // Initialize mass matrix
+- dMassSetCylinderTotal(&this->mass, this->massP->GetValue(), 3,
+- this->sizeP->GetValue().x, this->sizeP->GetValue().y);
+-
+ // create a cylinder geometry
-+ this->SetGeom( dCreateCylinder( 0, radius, length ), true );
-+
- // Initialize mass matrix
-- dMassSetCylinderTotal(&this->mass, this->dblMass, 3, radius, length);
+ this->SetGeom( dCreateCylinder( 0, this->sizeP->GetValue().x,
+ this->sizeP->GetValue().y ), true );
+
++ // Initialize mass matrix
+ // pending a tag <massMatrix>true</massMatrix> in geom:
-+ if (this->massMatrix)
-+ dMassSetParameters(&this->mass, this->dblMass, this->cx, this->cy, this->cz,
-+ this->ixx,this->iyy,this->izz,this->ixy,this->ixz,this->iyz);
++ if (this->customMassMatrix)
++ dMassSetParameters(&this->mass, this->massP->GetValue(),
++ this->cx, this->cy, this->cz,
++ this->ixx,this->iyy,this->izz,
++ this->ixy,this->ixz,this->iyz);
+ else
-+ dMassSetCylinderTotal(&this->mass, this->dblMass, 3, radius, length);
++ dMassSetCylinderTotal(&this->mass, this->massP->GetValue(), 3,
++ this->sizeP->GetValue().x, this->sizeP->GetValue().y);
++
++
+ }
-- this->SetGeom( dCreateCylinder( 0, radius, length ), true );
-
- //to be able to show physics
- /*this->visualNode->AttachMesh("unit_cylinder");
+ //////////////////////////////////////////////////////////////////////////////
Index: server/physics/Geom.cc
===================================================================
---- server/physics/Geom.cc (revision 6952)
+--- server/physics/Geom.cc (revision 6981)
+++ server/physics/Geom.cc (working copy)
-@@ -102,6 +102,21 @@
- this->dblMass = 0.001;
+@@ -66,6 +66,17 @@
+ this->rpyP = new Param<Quatern>("rpy", Quatern(), 0);
+ this->laserFiducialIdP = new Param<int>("laserFiducialId",-1,0);
+ this->laserRetroP = new Param<float>("laserRetro",-1,0);
++
++ this->customMassMatrixP = new Param<bool>("massMatrix",false,0);
++ this->cxP = new Param<double>("cx",0.0,0);
++ this->cyP = new Param<double>("cy",0.0,0);
++ this->czP = new Param<double>("cz",0.0,0);
++ this->ixxP = new Param<double>("ixx",1e-6,0);
++ this->iyyP = new Param<double>("iyy",1e-6,0);
++ this->izzP = new Param<double>("izz",1e-6,0);
++ this->ixyP = new Param<double>("ixy",0.0,0);
++ this->ixzP = new Param<double>("ixz",0.0,0);
++ this->iyzP = new Param<double>("iyz",0.0,0);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -91,6 +102,16 @@
+ delete this->rpyP;
+ delete this->laserFiducialIdP;
+ delete this->laserRetroP;
++ delete this->customMassMatrixP;
++ delete this->cxP ;
++ delete this->cyP ;
++ delete this->czP ;
++ delete this->ixxP;
++ delete this->iyyP;
++ delete this->izzP;
++ delete this->ixyP;
++ delete this->ixzP;
++ delete this->iyzP;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -115,6 +136,30 @@
+ this->massP->SetValue( 0.001 );
}
++ this->customMassMatrixP->Load(node);
++ this->cxP ->Load(node);
++ this->cyP ->Load(node);
++ this->czP ->Load(node);
++ this->ixxP->Load(node);
++ this->iyyP->Load(node);
++ this->izzP->Load(node);
++ this->ixyP->Load(node);
++ this->ixzP->Load(node);
++ this->iyzP->Load(node);
++
+ // option to enter full maxx matrix
-+ std::string tmpMassMatrix = node->GetString("massMatrix","false",0);
-+ if (tmpMassMatrix == "true") this->massMatrix = true;
-+ else this->massMatrix = false;
-+ this->cx = node->GetDouble("cx",0.0,0);
-+ this->cy = node->GetDouble("cy",0.0,0);
-+ this->cz = node->GetDouble("cz",0.0,0);
-+ this->ixx = node->GetDouble("ixx",0.001,0);
-+ this->iyy = node->GetDouble("iyy",0.001,0);
-+ this->izz = node->GetDouble("izz",0.001,0);
-+ this->ixy = node->GetDouble("ixy",0.001,0);
-+ this->ixz = node->GetDouble("ixz",0.001,0);
-+ this->iyz = node->GetDouble("iyz",0.001,0);
++ this->customMassMatrix = this->customMassMatrixP->GetValue();
++ this->cx = this->cxP ->GetValue();
++ this->cy = this->cyP ->GetValue();
++ this->cz = this->czP ->GetValue();
++ this->ixx = this->ixxP->GetValue();
++ this->iyy = this->iyyP->GetValue();
++ this->izz = this->izzP->GetValue();
++ this->ixy = this->ixyP->GetValue();
++ this->ixz = this->ixzP->GetValue();
++ this->iyz = this->iyzP->GetValue();
+
+
this->contact->Load(node);
this->LoadChild(node);
-@@ -372,18 +387,21 @@
+@@ -393,18 +438,21 @@
if (!this->placeable)
return NULL;
@@ -756,49 +739,47 @@
dMassRotate(&this->bodyMass, r);
Index: server/physics/HingeJoint.cc
===================================================================
---- server/physics/HingeJoint.cc (revision 6952)
+--- server/physics/HingeJoint.cc (revision 6981)
+++ server/physics/HingeJoint.cc (working copy)
-@@ -38,6 +38,7 @@
- : Joint()
- {
- this->jointId = dJointCreateHinge( worldId, NULL );
-+ this->type = Joint::HINGE;
+@@ -43,6 +43,7 @@
+ this->axisP = new Param<Vector3>("axis",Vector3(0,1,0), 1);
+ this->loStopP = new Param<Angle>("lowStop",-M_PI,0);
+ this->hiStopP = new Param<Angle>("highStop",M_PI,0);
++ this->anchorOffsetP = new Param<Vector3>("anchorOffset",Vector3(0,0,0),0);
}
-@@ -55,6 +56,7 @@
+@@ -62,6 +63,7 @@
+ this->axisP->Load(node);
+ this->loStopP->Load(node);
+ this->hiStopP->Load(node);
++ this->anchorOffsetP->Load(node);
- double loStop = DTOR(node->GetDouble("lowStop",RTOD(-M_PI),0));
- double hiStop = DTOR(node->GetDouble("highStop",RTOD(M_PI),0));
-+ anchorOffset = node->GetVector3("axisOffset",Vector3(0,0,0));
-
// Perform this three step ordering to ensure the parameters are set
// properly. This is taken from the ODE wiki.
-@@ -115,7 +117,7 @@
+@@ -131,7 +133,7 @@
// Set the anchor point
void HingeJoint::SetAnchor( const Vector3 &anchor )
{
- dJointSetHingeAnchor( this->jointId, anchor.x, anchor.y, anchor.z );
-+ dJointSetHingeAnchor( this->jointId, anchor.x+anchorOffset.x, anchor.y+anchorOffset.y, anchor.z+anchorOffset.z );
++ dJointSetHingeAnchor( this->jointId, anchor.x+anchorOffsetP->GetValue().x, anchor.y+anchorOffsetP->GetValue().y, anchor.z+anchorOffsetP->GetValue().z );
}
Index: server/physics/ode/ODEPhysics.cc
===================================================================
---- server/physics/ode/ODEPhysics.cc (revision 6952)
+--- server/physics/ode/ODEPhysics.cc (revision 6981)
+++ server/physics/ode/ODEPhysics.cc (working copy)
-@@ -259,18 +259,18 @@
- double h, kp, kd;
-
- contact.geom = contactGeoms[i];
-- contact.surface.mode = dContactSoftERP | dContactBounce | dContactMu2;
-+ //contact.surface.mode = dContactSoftERP | dContactBounce | dContactMu2;
+@@ -264,15 +264,16 @@
+ contact.surface.mode = dContactSlip1 | dContactSlip2 |
+ dContactSoftERP | dContactSoftCFM |
+ dContactBounce | dContactMu2 | dContactApprox1;
+ contact.surface.mode = 0;
// Compute the CFM and ERP by assuming the two bodies form a
// spring-damper system.
- h = self->stepTime;
+ h = self->stepTimeP->GetValue();
- kp = 1 / (1 / geom1->contact->kp + 1 / geom2->contact->kp);
+ kp = 1.0 / (1.0 / geom1->contact->kp + 1.0 / geom2->contact->kp);
kd = geom1->contact->kd + geom2->contact->kd;
@@ -806,45 +787,32 @@
- contact.surface.soft_cfm = 1 / (h * kp + kd);
+ contact.surface.soft_cfm = 1.0 / (h * kp + kd);
--
contact.surface.mu = MIN(geom1->contact->mu1, geom2->contact->mu1);
contact.surface.mu2 = MIN(geom1->contact->mu2, geom2->contact->mu2);
- contact.surface.bounce = 0.1;
Index: server/physics/TrimeshGeom.cc
===================================================================
---- server/physics/TrimeshGeom.cc (revision 6952)
+--- server/physics/TrimeshGeom.cc (revision 6981)
+++ server/physics/TrimeshGeom.cc (working copy)
-@@ -208,7 +208,11 @@
+@@ -206,7 +206,13 @@
this->geomId = dCreateTriMesh( this->spaceId, this->odeData,0,0,0 );
-- dMassSetTrimesh(&this->mass, this->dblMass, this->geomId);
-+ if (this->massMatrix)
-+ dMassSetParameters(&this->mass, this->dblMass, this->cx, this->cy, this->cz,
-+ this->ixx,this->iyy,this->izz,this->ixy,this->ixz,this->iyz);
+- dMassSetTrimesh(&this->mass, this->massP->GetValue(), this->geomId);
++ if (this->customMassMatrix)
++ dMassSetParameters(&this->mass, this->massP->GetValue(),
++ this->cx, this->cy, this->cz,
++ this->ixx,this->iyy,this->izz,
++ this->ixy,this->ixz,this->iyz);
+ else
-+ dMassSetTrimesh(&this->mass, this->dblMass, this->geomId);
++ dMassSetTrimesh(&this->mass, this->massP->GetValue(), this->geomId);
// Create the trimesh geometry
this->SetGeom(this->geomId, true);
-Index: server/physics/SliderJoint.cc
-===================================================================
---- server/physics/SliderJoint.cc (revision 6952)
-+++ server/physics/SliderJoint.cc (working copy)
-@@ -35,6 +35,8 @@
- : Joint()
- {
- this->jointId = dJointCreateSlider( worldId, NULL );
-+ this->type = Joint::SLIDER;
-+ fprintf(stderr," slider jointId %d\n",this->jointId);
- }
-
-
Index: server/sensors/Sensor.hh
===================================================================
---- server/sensors/Sensor.hh (revision 6952)
+--- server/sensors/Sensor.hh (revision 6981)
+++ server/sensors/Sensor.hh (working copy)
-@@ -63,6 +63,7 @@
+@@ -70,6 +70,7 @@
/// \brief Set whether the sensor is active or not
public: void SetActive(bool value);
@@ -852,283 +820,19 @@
/// \brief Load the child sensor
protected: virtual void LoadChild(XMLConfigNode * /*node*/) {};
-@@ -88,8 +89,14 @@
-
- /// \brief True if active
- protected: bool active;
-+
-+ /// \brief Update period
-+ protected: double updatePeriod;
-+
-+ /// \brief Last update time
-+ protected: double lastUpdate;
-+
+@@ -101,7 +102,6 @@
+ protected: double lastUpdate;
+ protected: std::string typeName;
};
-
/// \}
}
#endif
-Index: server/sensors/camera/MonoCameraSensor.cc
-===================================================================
---- server/sensors/camera/MonoCameraSensor.cc (revision 6952)
-+++ server/sensors/camera/MonoCameraSensor.cc (working copy)
-@@ -49,6 +49,7 @@
- MonoCameraSensor::MonoCameraSensor(Body *body)
- : Sensor(body), OgreCamera("Mono")
- {
-+ this->active = false;
- }
-
-
-@@ -120,46 +121,50 @@
- // Update the drawing
- void MonoCameraSensor::UpdateChild()
- {
-- this->UpdateCam();
-
-- this->renderTarget->update();
--
-- Ogre::HardwarePixelBufferSharedPtr mBuffer;
-+ // Allocate buffer
- size_t size;
--
-- // Get access to the buffer and make an image and write it to file
-- mBuffer = this->renderTexture->getBuffer(0, 0);
--
- size = this->imageWidth * this->imageHeight * 3;
--
-- // Allocate buffer
- if (!this->saveFrameBuffer)
- this->saveFrameBuffer = new unsigned char[size];
-
-- mBuffer->lock(Ogre::HardwarePixelBuffer::HBL_READ_ONLY);
-+ if (this->active)
-+ {
-+ this->UpdateCam();
-
-- int top = (int)((mBuffer->getHeight() - this->imageHeight) / 2.0);
-- int left = (int)((mBuffer->getWidth() - this->imageWidth) / 2.0);
-- int right = left + this->imageWidth;
-- int bottom = top + this->imageHeight;
-+ this->renderTarget->update();
-
-- // Get the center of the texture in RGB 24 bit format
-- mBuffer->blitToMemory(
-- Ogre::Box(left, top, right, bottom),
-+ Ogre::HardwarePixelBufferSharedPtr mBuffer;
-
-- Ogre::PixelBox(
-- this->imageWidth,
-- this->imageHeight,
-- 1,
-- Ogre::PF_B8G8R8,
-- this->saveFrameBuffer)
-- );
-+ // Get access to the buffer and make an image and write it to file
-+ mBuffer = this->renderTexture->getBuffer(0, 0);
-
-- mBuffer->unlock();
-
-+ mBuffer->lock(Ogre::HardwarePixelBuffer::HBL_READ_ONLY);
-
-- if (this->saveFrames)
-- this->SaveFrame();
-+ int top = (int)((mBuffer->getHeight() - this->imageHeight) / 2.0);
-+ int left = (int)((mBuffer->getWidth() - this->imageWidth) / 2.0);
-+ int right = left + this->imageWidth;
-+ int bottom = top + this->imageHeight;
-+
-+ // Get the center of the texture in RGB 24 bit format
-+ mBuffer->blitToMemory(
-+ Ogre::Box(left, top, right, bottom),
-+
-+ Ogre::PixelBox(
-+ this->imageWidth,
-+ this->imageHeight,
-+ 1,
-+ Ogre::PF_B8G8R8,
-+ this->saveFrameBuffer)
-+ );
-+
-+ mBuffer->unlock();
-+
-+
-+ if (this->saveFrames)
-+ this->SaveFrame();
-+ }
- }
-
- ////////////////////////////////////////////////////////////////////////////////
-Index: server/sensors/camera/StereoCameraSensor.cc
-===================================================================
---- server/sensors/camera/StereoCameraSensor.cc (revision 6952)
-+++ server/sensors/camera/StereoCameraSensor.cc (working copy)
-@@ -58,6 +58,7 @@
- this->depthBuffer[1] = NULL;
- this->rgbBuffer[0] = NULL;
- this->rgbBuffer[1] = NULL;
-+ this->active = false;
- }
-
-
-@@ -195,87 +196,90 @@
- Ogre::SceneNode *gridNode = NULL;
- int i;
-
-- this->UpdateCam();
--
-- try
-+ if (this->active)
- {
-- gridNode = sceneMgr->getSceneNode("__OGRE_GRID_NODE__");
-- }
-- catch (...)
-- {
-- gridNode = NULL;
-- }
-+ this->UpdateCam();
-
-- sceneMgr->_suppressRenderStateChanges(true);
-+ try
-+ {
-+ gridNode = sceneMgr->getSceneNode("__OGRE_GRID_NODE__");
-+ }
-+ catch (...)
-+ {
-+ gridNode = NULL;
-+ }
-
-- //prev_pass = sceneMgr->getPass();
-+ sceneMgr->_suppressRenderStateChanges(true);
-
-- // Get pointer to the material pass
-- pass = this->depthMaterial->getBestTechnique()->getPass(0);
-+ //prev_pass = sceneMgr->getPass();
-
-- if (gridNode)
-- gridNode->setVisible(false);
-+ // Get pointer to the material pass
-+ pass = this->depthMaterial->getBestTechnique()->getPass(0);
-
-- // Render the depth texture
-- for (i=2; i<4; i++)
-- {
-+ if (gridNode)
-+ gridNode->setVisible(false);
-
-- // OgreSceneManager::_render function automatically sets farClip to 0.
-- // Which normally equates to infinite distance. We don't want this. So
-- // we have to set the distance every time.
-- this->GetOgreCamera()->setFarClipDistance( this->farClip );
-+ // Render the depth texture
-+ for (i=2; i<4; i++)
-+ {
-
-+ // OgreSceneManager::_render function automatically sets farClip to 0.
-+ // Which normally equates to infinite distance. We don't want this. So
-+ // we have to set the distance every time.
-+ this->GetOgreCamera()->setFarClipDistance( this->farClip );
-
-- Ogre::AutoParamDataSource autoParamDataSource;
-
-- vp = this->renderTargets[i]->getViewport(0);
-+ Ogre::AutoParamDataSource autoParamDataSource;
-
-- // Need this line to render the ground plane. No idea why it's necessary.
-- renderSys->_setViewport(vp);
-- sceneMgr->_setPass(pass, true, false);
-- autoParamDataSource.setCurrentPass(pass);
-- autoParamDataSource.setCurrentViewport(vp);
-- autoParamDataSource.setCurrentRenderTarget(this->renderTargets[i]);
-- autoParamDataSource.setCurrentSceneManager(sceneMgr);
-- autoParamDataSource.setCurrentCamera(this->GetOgreCamera());
-- pass->_updateAutoParamsNoLights(autoParamDataSource);
-+ vp = this->renderTargets[i]->getViewport(0);
-
-- // These two lines don't seem to do anything useful
-- renderSys->_setProjectionMatrix(this->GetOgreCamera()->getProjectionMatrixRS());
-- renderSys->_setViewMatrix(this->GetOgreCamera()->getViewMatrix(true));
-+ // Need this line to render the ground plane. No idea why it's necessary.
-+ renderSys->_setViewport(vp);
-+ sceneMgr->_setPass(pass, true, false);
-+ autoParamDataSource.setCurrentPass(pass);
-+ autoParamDataSource.setCurrentViewport(vp);
-+ autoParamDataSource.setCurrentRenderTarget(this->renderTargets[i]);
-+ autoParamDataSource.setCurrentSceneManager(sceneMgr);
-+ autoParamDataSource.setCurrentCamera(this->GetOgreCamera());
-+ pass->_updateAutoParamsNoLights(autoParamDataSource);
-
-- // NOTE: We MUST bind parameters AFTER updating the autos
-- if (pass->hasVertexProgram())
-+ // These two lines don't seem to do anything useful
-+ renderSys->_setProjectionMatrix(this->GetOgreCamera()->getProjectionMatrixRS());
-+ renderSys->_setViewMatrix(this->GetOgreCamera()->getViewMatrix(true));
-+
-+ // NOTE: We MUST bind parameters AFTER updating the autos
-+ if (pass->hasVertexProgram())
-+ {
-+ renderSys->bindGpuProgram( pass->getVertexProgram()->_getBindingDelegate() );
-+ ren...
[truncated message content] |
|
From: <tf...@us...> - 2008-08-26 18:57:54
|
Revision: 3626
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3626&view=rev
Author: tfoote
Date: 2008-08-26 18:58:03 +0000 (Tue, 26 Aug 2008)
Log Message:
-----------
recovering IBPS from unported and stripping out legacy code
Modified Paths:
--------------
pkg/trunk/nav/wavefront_player/wavefront_player.cc
Added Paths:
-----------
pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/
Removed Paths:
-------------
pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/doc/
pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/lib/
pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/nodes/
pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/rosbuild
pkg/trunk/unported/IBPSBatteryInterface/
Deleted: pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/rosbuild
===================================================================
--- pkg/trunk/unported/IBPSBatteryInterface/rosbuild 2008-07-14 05:35:02 UTC (rev 1526)
+++ pkg/trunk/drivers/robot/pr2/IBPSBatteryInterface/rosbuild 2008-08-26 18:58:03 UTC (rev 3626)
@@ -1,36 +0,0 @@
-#!/usr/bin/env ruby
-
-if !ENV['ROS_ROOT']
- puts "aaaaaaa! The ROS_ROOT environment variable is not set."
- puts "Navigate to your ROS_ROOT directory and run ./set_ros_root"
- puts "(alternatively, you can do this in your .bashrc file"
- exit
-end
-
-pkg_path = File.expand_path($0).split('/')
-pkg_path = pkg_path[0,pkg_path.length-1].join('/')
-
-if ARGV.length == 0 || ARGV[0] == 'update'
- puts "generating flows..."
- # look up the latest versions of rosrb and rospy
- rosrb = `#{ENV['ROS_ROOT']}/rospack latest rosrb`.chomp
- rospy = `#{ENV['ROS_ROOT']}/rospack latest rospy`.chomp
- puts `#{rospy}/scripts/flowgen_py #{pkg_path}/flows/*.flow`
- puts `#{rosrb}/scripts/flowgen_rb #{pkg_path}/flows/*.flow`
-elsif ARGV[0] == 'clean'
- puts "cleaning auto-generated flows..."
- # string hackery to generate the same filenames the flow generators do
- pkg = File.expand_path($0).split('/')[-2]
- Dir.glob("#{pkg_path}/flows/*.flow").each do |f|
- f = f.split('/')[-1]
- f[-5,:end] = ''
- puts "deleting auto-generated flows for flow #{f}"
- `rm #{pkg_path}/flows/python/#{pkg}/#{f}.py` if File.exist? "#{pkg_path}/flows/python/#{pkg}/#{f}.py"
- `rm #{pkg_path}/flows/python/#{pkg}/#{f}.pyc` if File.exist? "#{pkg_path}/flows/python/#{pkg}/#{f}.pyc"
- `rm #{pkg_path}/flows/ruby/#{pkg}/Flow#{f}.rb` if File.exist? "#{pkg_path}/flows/ruby/#{pkg}/Flow#{f}.rb"
- end
- `rm #{pkg_path}/flows/python/#{pkg}/__init__.py` if File.exist? "#{pkg_path}/flows/python/#{pkg}/__init__.py"
- `rm #{pkg_path}/flows/python/#{pkg}/__init__.pyc` if File.exist? "#{pkg_path}/flows/python/#{pkg}/__init__.pyc"
-else
- puts "unknown parameter: #{ARGV[0]}"
-end
Modified: pkg/trunk/nav/wavefront_player/wavefront_player.cc
===================================================================
--- pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-08-26 18:56:03 UTC (rev 3625)
+++ pkg/trunk/nav/wavefront_player/wavefront_player.cc 2008-08-26 18:58:03 UTC (rev 3626)
@@ -292,7 +292,7 @@
avmax(DTOR(80.0)),
amin(DTOR(10.0)),
amax(DTOR(40.0)),
- tf(*this, true, 1 * 1000000000ULL, 0ULL)
+ tf(*this, true, 1 * 1000000000ULL, 100000000ULL)
//tf(*this, true, 200000000ULL, 200000000ULL) //nanoseconds
{
// set a few parameters. leave defaults just as in the ctor initializer list
@@ -445,11 +445,13 @@
catch(libTF::TransformReference::LookupException& ex)
{
puts("no global->local Tx yet");
+ printf("%s\n", ex.what());
return;
}
catch(libTF::TransformReference::ExtrapolateException& ex)
{
- // puts("extrapolation required");
+ puts("extrapolation required");
+ printf("%s\n", ex.what());
continue;
}
@@ -591,6 +593,7 @@
{
// this should never happen
puts("WARNING: extrapolation failed!");
+ printf("%s",ex.what());
this->stopRobot();
return;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-26 20:31:41
|
Revision: 3634
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3634&view=rev
Author: isucan
Date: 2008-08-26 20:31:49 +0000 (Tue, 26 Aug 2008)
Log Message:
-----------
fixed some of the self-collision groups, added ground for collision checking, added option to limit the bodies that are tested for collision
Modified Paths:
--------------
pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h
pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h
pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h
pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environment.h 2008-08-26 20:31:49 UTC (rev 3634)
@@ -73,9 +73,12 @@
/** Add a point cloud to the collision space */
virtual void addPointCloud(unsigned int n, const double* points, double radius = 0.01) = 0;
- /** Add a robot model */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model);
+ /** Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
+ virtual void addPlane(double a, double b, double c, double d) = 0;
+ /** Add a robot model. Ignore robot links if their name is not specified in the string vector */
+ virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links);
+
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id) = 0;
@@ -84,6 +87,7 @@
/** Get the number of loaded models */
unsigned int getModelCount(void) const;
+
/** Get a specific model */
planning_models::KinematicModel* getRobotModel(unsigned int model_id) const;
@@ -107,8 +111,6 @@
/** List of loaded robot models */
std::vector<planning_models::KinematicModel*> m_models;
-
-
};
}
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentODE.h 2008-08-26 20:31:49 UTC (rev 3634)
@@ -81,9 +81,12 @@
/** Add a point cloud to the collision space */
virtual void addPointCloud(unsigned int n, const double *points, double radius = 0.01);
- /** Add a robot model */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model);
+ /** Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
+ virtual void addPlane(double a, double b, double c, double d);
+ /** Add a robot model. Ignore robot links if their name is not specified in the string vector */
+ virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links);
+
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id);
@@ -175,6 +178,7 @@
std::vector<ModelInfo> m_kgeoms;
dSpaceID m_space;
ODECollide2 m_collide2;
+ std::vector<dGeomID> m_odeGeoms;
};
}
Modified: pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h
===================================================================
--- pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/collision_space/include/collision_space/environmentOctree.h 2008-08-26 20:31:49 UTC (rev 3634)
@@ -75,9 +75,12 @@
/** Add a point cloud to the collision space */
virtual void addPointCloud(unsigned int n, const double *points, double radius = 0.01);
- /** Add a robot model */
- virtual unsigned int addRobotModel(planning_models::KinematicModel *model);
+ /** Add a plane to the collision space. Equation it satisfies is a*x+b*y+c*z = d*/
+ virtual void addPlane(double a, double b, double c, double d);
+ /** Add a robot model. Ignore robot links if their name is not specified in the string vector */
+ virtual unsigned int addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links);
+
/** Update the positions of the geometry used in collision detection */
virtual void updateRobotModel(unsigned int model_id);
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environment.cpp 2008-08-26 20:31:49 UTC (rev 3634)
@@ -36,7 +36,7 @@
#include <collision_space/environment.h>
-unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model)
+unsigned int collision_space::EnvironmentModel::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links)
{
unsigned int pos = m_models.size();
m_models.push_back(model);
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-08-26 20:31:49 UTC (rev 3634)
@@ -35,6 +35,7 @@
/** \Author Ioan Sucan */
#include <collision_space/environmentODE.h>
+#include <map>
void collision_space::EnvironmentModelODE::freeMemory(void)
{
@@ -48,21 +49,33 @@
dSpaceDestroy(m_space);
}
-unsigned int collision_space::EnvironmentModelODE::addRobotModel(planning_models::KinematicModel *model)
+unsigned int collision_space::EnvironmentModelODE::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links)
{
- unsigned int id = collision_space::EnvironmentModel::addRobotModel(model);
-
+ unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links);
+
if (m_kgeoms.size() <= id)
{
m_kgeoms.resize(id + 1);
m_kgeoms[id].space = dHashSpaceCreate(0);
}
+
+ std::map<std::string, bool> exists;
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ exists[links[i]] = true;
for (unsigned int j = 0 ; j < m_models[id]->getRobotCount() ; ++j)
{
planning_models::KinematicModel::Robot *robot = m_models[id]->getRobot(j);
for (unsigned int i = 0 ; i < robot->links.size() ; ++i)
{
+ /* skip this link if we have no geometry or if the link
+ name is not specified as enabled for collision
+ checking */
+ if (!robot->links[i]->shape)
+ continue;
+ if (exists.find(robot->links[i]->name) == exists.end())
+ continue;
+
kGeom *kg = new kGeom();
kg->link = robot->links[i];
dGeomID g = createODEGeom(m_kgeoms[id].space, robot->links[i]->shape);
@@ -217,6 +230,7 @@
CollisionData cdata;
cdata.collides = false;
+ /* check self collision */
if (m_selfCollision)
{
for (int i = m_kgeoms[model_id].selfCollision.size() - 1 ; i >= 0 ; --i)
@@ -244,19 +258,49 @@
if (cdata.collides)
{
std::cout << m_kgeoms[model_id].geom[vec[j]]->link->name << " intersects with " << m_kgeoms[model_id].geom[vec[k]]->link->name << std::endl;
- goto OUT;
+ goto OUT1;
}
}
}
}
- OUT:
+ /* check collision with standalone ode bodies */
+ OUT1:
+
if (!cdata.collides)
{
+ for (int i = m_kgeoms[model_id].geom.size() - 1 ; i >= 0 ; --i)
+ {
+ dGeomID g1 = m_kgeoms[model_id].geom[i]->geom;
+ dReal aabb1[6];
+ dGeomGetAABB(g1, aabb1);
+ for (int j = m_odeGeoms.size() - 1 ; j >= 0 ; --j)
+ {
+ dGeomID g2 = m_odeGeoms[j];
+ dReal aabb2[6];
+ dGeomGetAABB(g2, aabb2);
+ if (!(aabb1[2] > aabb2[3] ||
+ aabb1[3] < aabb2[2] ||
+ aabb1[4] > aabb2[5] ||
+ aabb1[5] < aabb2[4]))
+ dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ if (cdata.collides)
+ goto OUT2;
+ }
+ }
+ }
+
+ /* check collision with pointclouds */
+ OUT2:
+
+ if (!cdata.collides)
+ {
m_collide2.setup();
for (int i = m_kgeoms[model_id].geom.size() - 1 ; i >= 0 && !cdata.collides ; --i)
m_collide2.collide(m_kgeoms[model_id].geom[i]->geom, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+ if (cdata.collides)
+ std::cout << "Pointcloud intersection" << std::endl;
}
return cdata.collides;
@@ -274,6 +318,12 @@
m_collide2.setup();
}
+void collision_space::EnvironmentModelODE::addPlane(double a, double b, double c, double d)
+{
+ dGeomID g = dCreatePlane(m_space, a, b, c, d);
+ m_odeGeoms.push_back(g);
+}
+
void collision_space::EnvironmentModelODE::clearObstacles(void)
{
m_collide2.clear();
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentOctree.cpp 2008-08-26 20:31:49 UTC (rev 3634)
@@ -36,12 +36,22 @@
#include <collision_space/environmentOctree.h>
-unsigned int collision_space::EnvironmentModelOctree::addRobotModel(planning_models::KinematicModel *model)
+unsigned int collision_space::EnvironmentModelOctree::addRobotModel(planning_models::KinematicModel *model, const std::vector<std::string> &links)
{
- unsigned int id = collision_space::EnvironmentModel::addRobotModel(model);
+ unsigned int id = collision_space::EnvironmentModel::addRobotModel(model, links);
if (id <= m_modelLinks.size())
m_modelLinks.resize(id + 1);
- m_models[id]->getLinks(m_modelLinks[id]);
+
+ std::vector< planning_models::KinematicModel::Link*> allLinks;
+
+ std::map<std::string, bool> exists;
+ for (unsigned int i = 0 ; i < links.size() ; ++i)
+ exists[links[i]] = true;
+
+ for (unsigned int i = 0 ; i < allLinks.size() ; ++i)
+ if (exists.find(allLinks[i]->name) != exists.end())
+ m_modelLinks[id].push_back(allLinks[i]);
+
return id;
}
@@ -138,6 +148,11 @@
return &m_octree;
}
+void collision_space::EnvironmentModelOctree::addPlane(double a, double b, double c, double d)
+{
+ fprintf(stderr, "Octree collision checking does not support planes\n");
+}
+
void collision_space::EnvironmentModelOctree::addSelfCollisionGroup(unsigned int model_id, std::vector<std::string> &links)
{
fprintf(stderr, "Octree collision checking does not support self collision\n");
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-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-26 20:31:49 UTC (rev 3634)
@@ -190,10 +190,11 @@
req.goal_constraints[0].robot_link = "wrist_flex_left";
req.goal_constraints[0].pose.position.x = 0.5;
req.goal_constraints[0].pose.position.y = 0.3;
- req.goal_constraints[0].pose.position.z = 1.0;
+ req.goal_constraints[0].pose.position.z = -1.0;
req.goal_constraints[0].position_distance = 0.01;
// an example of constraints: do not move the elbow too much
+ /*
req.constraints.set_pose_size(1);
req.constraints.pose[0].type = robot_msgs::PoseConstraint::ONLY_POSITION;
req.constraints.pose[0].robot_link = "elbow_flex_left";
@@ -201,7 +202,7 @@
req.constraints.pose[0].pose.position.y = 0.188;
req.constraints.pose[0].pose.position.z = 0.74;
req.constraints.pose[0].position_distance = 0.01;
-
+ */
req.allowed_time = 3.0;
req.params.volumeMin.x = -5.0 + m_basePos[0]; req.params.volumeMin.y = -5.0 + m_basePos[1]; req.params.volumeMin.z = 0.0;
Modified: pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/planning_models/src/planning_models/kinematic.cpp 2008-08-26 20:31:49 UTC (rev 3634)
@@ -496,14 +496,10 @@
{
unsigned int pos = m_pos[name];
for (unsigned int i = 0 ; i < joint->usedParams ; ++i)
- {
- std::cout << joint->name << ": Setting param " << i << " to " << params[i] << std::endl;
m_params[pos + i] = params[i];
- }
-
}
else
- fprintf(stderr, "Unknown joint: '%s'\n", name.c_str());
+ std::cerr << "Unknown joint: '" << name << "'" << std::endl;
}
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/cnode.h 2008-08-26 20:31:49 UTC (rev 3634)
@@ -94,6 +94,7 @@
else
m_collisionSpace = new collision_space::EnvironmentModelODE();
m_collisionSpace->setSelfCollision(true);
+ m_collisionSpace->addPlane(0.0, 0.0, 1.0, -0.01);
m_sphereSize = 0.03;
@@ -114,8 +115,12 @@
NodeRobotModel::setRobotDescription(file);
if (m_kmodel)
{
+ std::vector<std::string> links;
+ robot_desc::URDF::Group *g = file->getGroup("collision_check");
+ if (g && g->hasFlag("collision"))
+ links = g->linkNames;
m_collisionSpace->lock();
- unsigned int cid = m_collisionSpace->addRobotModel(m_kmodel);
+ unsigned int cid = m_collisionSpace->addRobotModel(m_kmodel, links);
m_collisionSpace->unlock();
addSelfCollisionGroups(cid, file);
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups.xml 2008-08-26 20:31:49 UTC (rev 3634)
@@ -4,7 +4,7 @@
<!-- Define the parts of the robot that the robot's scanners can see -->
- <group name="self_see" flags="mapping">
+ <group name="self_see" flags="collision">
upperarm_roll_left
upperarm_roll_right
elbow_flex_left
@@ -21,97 +21,82 @@
finger_r_right
</group>
- <!-- Define groups to check for self collision -->
-
- <group name="collisions1" flags="self_collision">
- shoulder_pan_right
+ <!-- Define the parts of the robot that the collision checker should care about -->
+ <group name="collision_check" flags="collision">
+ base
torso
- </group>
-
-
- <group name="collisions2" flags="self_collision">
+ shoulder_pan_right
shoulder_pitch_right
- torso
+ shoulder_pan_left
+ shoulder_pitch_left
+ upperarm_roll_left
+ upperarm_roll_right
+ elbow_flex_left
+ elbow_flex_right
+ forearm_roll_left
+ forearm_roll_right
+ wrist_flex_left
+ wrist_flex_right
+ gripper_roll_left
+ gripper_roll_right
+ finger_l_left
+ finger_r_left
+ finger_l_right
+ finger_r_right
+ head_pan
+ head_tilt
</group>
+
+ <!-- Define groups to check for self collision -->
- <group name="collisions3" flags="self_collision">
+ <group name="collisions1" flags="self_collision">
upperarm_roll_right
torso
</group>
- <group name="collisions4" flags="self_collision">
+ <group name="collisions2" flags="self_collision">
elbow_flex_right
base
</group>
- <group name="collisions5" flags="self_collision">
+ <group name="collisions3" flags="self_collision">
forearm_roll_right
base
</group>
- <group name="collisions6" flags="self_collision">
+ <group name="collisions4" flags="self_collision">
wrist_flex_right
base
</group>
- <group name="collisions7" flags="self_collision">
+ <group name="collisions5" flags="self_collision">
gripper_roll_right
base
</group>
- <group name="collisions8" flags="self_collision">
- upperarm_roll_right
- shoulder_pan_left
- shoulder_pitch_left
+ <group name="collisions6" flags="self_collision">
upperarm_roll_left
- elbow_flex_left
- forearm_roll_left
- wrist_flex_left
- gripper_roll_left
+ torso
</group>
- <group name="collisions9" flags="self_collision">
- elbow_flex_right
- shoulder_pan_left
- shoulder_pitch_left
- upperarm_roll_left
+ <group name="collisions7" flags="self_collision">
elbow_flex_left
- forearm_roll_left
- wrist_flex_left
- gripper_roll_left
+ base
</group>
- <group name="collisions10" flags="self_collision">
- forearm_roll_right
- shoulder_pan_left
- shoulder_pitch_left
- upperarm_roll_left
- elbow_flex_left
+ <group name="collisions8" flags="self_collision">
forearm_roll_left
- wrist_flex_left
- gripper_roll_left
+ base
</group>
- <group name="collisions11" flags="self_collision">
- wrist_flex_right
- shoulder_pan_left
- shoulder_pitch_left
- upperarm_roll_left
- elbow_flex_left
- forearm_roll_left
+ <group name="collisions9" flags="self_collision">
wrist_flex_left
- gripper_roll_left
+ base
</group>
- <group name="collisions12" flags="self_collision">
- gripper_roll_right
- shoulder_pan_left
- shoulder_pitch_left
- upperarm_roll_left
- elbow_flex_left
- forearm_roll_left
- wrist_flex_left
+ <group name="collisions10" flags="self_collision">
gripper_roll_left
+ base
</group>
Modified: pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h
===================================================================
--- pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/util/display_ode_spaces/include/display_ode/displayODE.h 2008-08-26 20:31:49 UTC (rev 3634)
@@ -94,6 +94,8 @@
case dCylinderClass:
drawCylinder(geom);
break;
+ case dPlaneClass:
+ break;
default:
printf("Geometry class %d not yet implemented\n", cls);
break;
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-26 20:19:00 UTC (rev 3633)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-26 20:31:49 UTC (rev 3634)
@@ -267,7 +267,7 @@
void addSelfSeeBodies(void)
{
robot_desc::URDF::Group *ss = m_urdf->getGroup("self_see");
- if (ss && ss->hasFlag("mapping"))
+ if (ss && ss->hasFlag("collision"))
{
for (unsigned int i = 0 ; i < ss->linkNames.size() ; ++i)
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-08-26 20:52:29
|
Revision: 3637
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3637&view=rev
Author: stuglaser
Date: 2008-08-26 20:52:36 +0000 (Tue, 26 Aug 2008)
Log Message:
-----------
Parses kinematic chains and performs forward kinematics during update loop.
Modified Paths:
--------------
pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
pkg/trunk/mechanism/mechanism_model/manifest.xml
pkg/trunk/mechanism/mechanism_model/src/link.cpp
pkg/trunk/mechanism/mechanism_model/src/robot.cpp
pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/parser.h
pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp
Modified: pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_control/src/mechanism_control.cpp 2008-08-26 20:52:36 UTC (rev 3637)
@@ -91,13 +91,13 @@
// Propagates through the robot model.
for (unsigned int i = 0; i < model_.transmissions_.size(); ++i)
model_.transmissions_[i]->propagatePosition();
+ for (unsigned int i = 0; i < model_.chains_.size(); ++i)
+ model_.chains_[i]->propagateFK();
- //zero all commands
+ // Zeros all commands
for (unsigned int i = 0; i < model_.joints_.size(); ++i)
model_.joints_[i]->commanded_effort_= 0.0;
- // TODO: update KDL model with new joint position/velocities
-
// Update all controllers
for (int i = 0; i < MAX_NUM_CONTROLLERS; ++i)
{
Modified: pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-08-26 20:52:36 UTC (rev 3637)
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(mechanism_model)
-rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp src/robot.cpp src/link.cpp)
+rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp src/robot.cpp src/link.cpp src/chain.cpp)
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/link.h 2008-08-26 20:52:36 UTC (rev 3637)
@@ -36,6 +36,7 @@
#include "mechanism_model/joint.h"
#include <vector>
+#include <kdl/frames.hpp>
namespace mechanism {
@@ -59,6 +60,8 @@
Joint *joint_;
std::vector<Link*> children_;
+ KDL::Frame frame_;
+
private:
enum InitState {INIT_XML, CREATE_TREE_LINKS, INITIALIZED};
InitState init_state_;
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/robot.h 2008-08-26 20:52:36 UTC (rev 3637)
@@ -31,8 +31,27 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
-//The robot model is populated by the control code infrastructure and used by all the controllers to read mechanism state and command mechanism motion.
+/*
+ * The robot model tracks the state of the robot.
+ *
+ * State path:
+ * +---------------+ +--------+
+ * Actuators --> | Transmissions | --> Joints --> | Chains | --> Links
+ * +---------------+ +--------+
+ *
+ * The actuators, joints, and links, hold the state information. The
+ * actuators hold the encoder info, the joints hold the joint angles
+ * and velocities, and the links hold the frame transforms of the body
+ * segments.
+ *
+ * The transmissions and chains are for propagating the state through
+ * the model, and they themselves do not hold any information on the
+ * robot's current state.
+ *
+ * Author: Stuart Glaser
+ */
+
#ifndef ROBOT_H
#define ROBOT_H
@@ -43,6 +62,7 @@
#include "mechanism_model/link.h"
#include "mechanism_model/joint.h"
#include "mechanism_model/transmission.h"
+#include "mechanism_model/chain.h"
#include "hardware_interface/hardware_interface.h"
class TiXmlElement;
@@ -64,16 +84,16 @@
bool initXml(TiXmlElement *root);
+ HardwareInterface *hw_; // Holds the array of actuators
+ std::vector<Transmission*> transmissions_;
std::vector<Joint*> joints_;
- std::vector<Transmission*> transmissions_;
+ std::vector<Chain*> chains_;
std::vector<Link*> links_;
Joint* getJoint(const std::string &name);
Actuator* getActuator(const std::string &name);
Link* getLink(const std::string &name);
- HardwareInterface *hw_;
-
// For debugging
void printLinkTree();
};
Modified: pkg/trunk/mechanism/mechanism_model/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_model/manifest.xml 2008-08-26 20:52:36 UTC (rev 3637)
@@ -8,6 +8,8 @@
<depend package="stl_utils" />
<depend package="misc_utils" />
<depend package="tinyxml" />
+<depend package="wg_robot_description_parser" />
+<depend package="kdl" />
<url>http://pr.willowgarage.com</url>
<export>
<cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -lmechanism_model -Wl,-rpath,${prefix}/lib"/>
Modified: pkg/trunk/mechanism/mechanism_model/src/link.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/link.cpp 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_model/src/link.cpp 2008-08-26 20:52:36 UTC (rev 3637)
@@ -40,6 +40,7 @@
#include <map>
#include <string>
#include "mechanism_model/robot.h"
+#include "urdf/parser.h"
namespace mechanism {
@@ -64,6 +65,7 @@
}
name_ = std::string(name);
+ // Parent
TiXmlElement *p = config->FirstChildElement("parent");
const char *parent_name = p ? p->Attribute("name") : NULL;
if (!parent_name)
@@ -73,16 +75,47 @@
}
parent_name_ = std::string(parent_name);
+ // Joint
TiXmlElement *j = config->FirstChildElement("joint");
const char *joint_name = j ? j->Attribute("name") : NULL;
joint_ = joint_name ? robot->getJoint(joint_name) : NULL;
if (!joint_)
{
- fprintf(stderr, "Error: Link \"%s\" could not find the joint named \"%s\"\n", name_.c_str(), joint_name);
+ fprintf(stderr, "Error: Link \"%s\" could not find the joint named \"%s\"\n",
+ name_.c_str(), joint_name);
return false;
}
- // TODO: parse origin info
+ // Origin
+ TiXmlElement *o = config->FirstChildElement("origin");
+ if (!o)
+ {
+ fprintf(stderr, "Error: Link \"%s\" has no origin element\n", name_.c_str());
+ return false;
+ }
+ std::vector<double> xyz, rpy;
+ if (!urdf::queryVectorAttribute(o, "xyz", &xyz))
+ {
+ fprintf(stderr, "Error: Link \"%s\"'s origin has no xyz attribute\n", name_.c_str());
+ return false;
+ }
+ if (!urdf::queryVectorAttribute(o, "rpy", &rpy))
+ {
+ fprintf(stderr, "Error: Link \"%s\"'s origin has no rpy attribute\n", name_.c_str());
+ return false;
+ }
+ if (xyz.size() != 3)
+ {
+ fprintf(stderr, "Error: Link \"%s\"'s xyz origin is malformed\n", name_.c_str());
+ return false;
+ }
+ if (rpy.size() != 3)
+ {
+ fprintf(stderr, "Error: Link \"%s\"'s rpy origin is malformed\n", name_.c_str());
+ return false;
+ }
+ frame_ = KDL::Frame(KDL::Rotation::RPY(rpy[0], rpy[1], rpy[2]), KDL::Vector(xyz[0], xyz[1], xyz[2]));
+
// TODO: parse inertial info
// TODO: maybe parse collision info
Modified: pkg/trunk/mechanism/mechanism_model/src/robot.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/robot.cpp 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/mechanism/mechanism_model/src/robot.cpp 2008-08-26 20:52:36 UTC (rev 3637)
@@ -38,6 +38,7 @@
bool Robot::initXml(TiXmlElement *root)
{
+ assert(hw_);
TiXmlElement *xit = NULL;
// Constructs the joints.
@@ -81,18 +82,16 @@
}
printLinkTree();
-#if 0
// Constructs the kinematic chains.
for (xit = root->FirstChildElement("chain"); xit;
xit = xit->NextSiblingElement("chain"))
{
- kinematics::Chain *c = new kinematics::Chain;
+ Chain *c = new Chain;
if (c->initXml(xit, this))
chains_.push_back(c);
else
delete c;
}
-#endif
return true;
}
@@ -117,6 +116,7 @@
Actuator* Robot::getActuator(const std::string &name)
{
+ assert(hw_);
// Yeah, it's a linear search. Deal with it.
return findByName(hw_->actuators_, name);
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/parser.h
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/parser.h 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/include/urdf/parser.h 2008-08-26 20:52:36 UTC (rev 3637)
@@ -34,6 +34,7 @@
#define URDF_FUNCTIONAL_PARSER_H
#include "tinyxml/tinyxml.h"
+#include "string_utils/string_utils.h"
namespace urdf {
@@ -42,6 +43,8 @@
TRANSMISSIONS = 2
};
+bool queryVectorAttribute(TiXmlElement *el, const char *name, std::vector<double> *value);
+
// Destructively modifies the document, replacing references to consts
// and blocks with the actual values.
bool normalizeXml(TiXmlElement *elt);
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp 2008-08-26 20:39:41 UTC (rev 3636)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/src/parser.cpp 2008-08-26 20:52:36 UTC (rev 3637)
@@ -32,6 +32,7 @@
*/
#include "urdf/parser.h"
#include <map>
+#include <vector>
#include <cmath>
#include <sstream>
#include "tinyxml/tinyxml.h"
@@ -42,6 +43,21 @@
namespace urdf {
+bool queryVectorAttribute(TiXmlElement *el, const char *name, std::vector<double> *value)
+{
+ value->clear();
+ const char *s = el->Attribute(name);
+ if (!s)
+ return false;
+
+ std::vector<std::string> pieces;
+ string_utils::split(s, pieces);
+ for (unsigned int i = 0; i < pieces.size(); ++i)
+ value->push_back(atof(pieces[i].c_str()));
+
+ return true;
+}
+
// Pulls the const and const_block elements out of the XML file.
class ConstsAndBlocks : public TiXmlVisitor
{
@@ -52,7 +68,7 @@
deleteValues(&blocks);
}
- virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *firstAttribute)
+ virtual bool VisitEnter(const TiXmlElement &elt, const TiXmlAttribute *)
{
if (elt.Value() == std::string("const"))
{
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-08-27 00:33:23
|
Revision: 3661
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3661&view=rev
Author: hsujohnhsu
Date: 2008-08-27 00:33:33 +0000 (Wed, 27 Aug 2008)
Log Message:
-----------
added flag for turning gravity off. experimental.
update printing for test_actuators.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
Added Paths:
-----------
pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_gazebo_actuators.xml
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-08-27 00:15:44 UTC (rev 3660)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-08-27 00:33:33 UTC (rev 3661)
@@ -600,6 +600,18 @@
private: Param<Vector3> *xyzP;
private: Param<Quatern> *rpyP;
+Index: server/physics/Body.hh
+===================================================================
+--- server/physics/Body.hh (revision 6981)
++++ server/physics/Body.hh (working copy)
+@@ -170,6 +170,7 @@
+
+ private: Param<Vector3> *xyzP;
+ private: Param<Quatern> *rpyP;
++ private: Param<bool> *turnGravityOffP;
+ };
+
+ /// \}
Index: server/physics/HingeJoint.hh
===================================================================
--- server/physics/HingeJoint.hh (revision 6981)
@@ -737,6 +749,27 @@
if (dMassCheck(&this->bodyMass))
{
dMassRotate(&this->bodyMass, r);
+Index: server/physics/Body.cc
+===================================================================
+--- server/physics/Body.cc (revision 6981)
++++ server/physics/Body.cc (working copy)
+@@ -66,6 +66,7 @@
+
+ this->xyzP = new Param<Vector3>("xyz", Vector3(), 0);
+ this->rpyP = new Param<Quatern>("rpy", Quatern(), 0);
++ this->turnGravityOffP = new Param<bool>("turnGravityOff", false, 0);
+ }
+
+
+@@ -129,7 +130,7 @@
+ }
+
+ // If no geoms are attached, then don't let gravity affect the body.
+- if (this->geoms.size()==0)
++ if (this->turnGravityOffP->GetValue() || this->geoms.size()==0)
+ {
+ this->SetGravityMode(false);
+ }
Index: server/physics/HingeJoint.cc
===================================================================
--- server/physics/HingeJoint.cc (revision 6981)
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-08-27 00:15:44 UTC (rev 3660)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/test_actuators.cpp 2008-08-27 00:33:33 UTC (rev 3661)
@@ -379,12 +379,12 @@
std::cout << " LoadChild controller name: " << *controller_name << " type " << *controller_type << std::endl;
// initialize controller
- std::cout << " adding to mc_ " ;
+ std::cout << " adding to mc_ " << *controller_name << "(" << *controller_type << ")" << std::endl;
mc_.spawnController(*controller_type,
*controller_name,
zit);
- std::cout << " adding to rmc_ " ;
+ std::cout << " adding to rmc_ " << *controller_name << "(" << *controller_type << ")" << std::endl;
rmc_.spawnController(*controller_type,
*controller_name,
zit);
Added: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_gazebo_actuators.xml
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_gazebo_actuators.xml (rev 0)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_gazebo_actuators.xml 2008-08-27 00:33:33 UTC (rev 3661)
@@ -0,0 +1 @@
+link ../../wg_robot_description/pr2/pr2_gazebo_actuators.xml
\ No newline at end of file
Property changes on: pkg/trunk/robot_descriptions/gazebo_robot_description/world/pr2_gazebo_actuators.xml
___________________________________________________________________
Added: svn:special
+ *
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-08-27 00:15:44 UTC (rev 3660)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/pr2.xml 2008-08-27 00:33:33 UTC (rev 3661)
@@ -1165,6 +1165,9 @@
<collision >
<insert_const_block name="pr2_shoulder_pan_collision"/>
</collision>
+ <map name="pr2_shoulder_pan_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<link name="shoulder_pan_right"><!-- link specifying the shoulder of the robot -->
@@ -1183,6 +1186,9 @@
<collision >
<insert_const_block name="pr2_shoulder_pan_collision"/>
</collision>
+ <map name="pr2_shoulder_pan_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
@@ -1202,6 +1208,9 @@
<collision >
<insert_const_block name="pr2_shoulder_pitch_collision"/>
</collision>
+ <map name="pr2_shoulder_pitch_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<link name="shoulder_pitch_right"><!-- link specifying the shoulder of the robot -->
@@ -1220,6 +1229,9 @@
<collision >
<insert_const_block name="pr2_shoulder_pitch_collision"/>
</collision>
+ <map name="pr2_shoulder_pitch_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
@@ -1238,6 +1250,9 @@
<collision >
<insert_const_block name="pr2_upperarm_roll_collision"/>
</collision>
+ <map name="pr2_upperarm_roll_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<link name="upperarm_roll_right"><!-- link specifying the shoulder of the robot -->
@@ -1256,6 +1271,9 @@
<collision >
<insert_const_block name="pr2_upperarm_roll_collision"/>
</collision>
+ <map name="pr2_upperarm_roll_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
@@ -1275,6 +1293,9 @@
<collision >
<insert_const_block name="pr2_elbow_flex_collision"/>
</collision>
+ <map name="pr2_elbow_flex_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<link name="elbow_flex_right"><!-- link specifying the shoulder of the robot -->
@@ -1293,6 +1314,9 @@
<collision >
<insert_const_block name="pr2_elbow_flex_collision"/>
</collision>
+ <map name="pr2_elbow_flex_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<link name="forearm_roll_left"><!-- link specifying the shoulder of the robot -->
@@ -1310,6 +1334,9 @@
<collision >
<insert_const_block name="pr2_forearm_roll_collision"/>
</collision>
+ <map name="pr2_forearm_roll_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<link name="forearm_roll_right"><!-- link specifying the shoulder of the robot -->
@@ -1327,6 +1354,9 @@
<collision >
<insert_const_block name="pr2_forearm_roll_collision"/>
</collision>
+ <map name="pr2_forearm_roll_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<link name="wrist_flex_left"><!-- link specifying the shoulder of the robot -->
@@ -1344,6 +1374,9 @@
<collision >
<insert_const_block name="pr2_wrist_flex_collision"/>
</collision>
+ <map name="pr2_wrist_flex_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<link name="wrist_flex_right"><!-- link specifying the shoulder of the robot -->
@@ -1362,6 +1395,9 @@
<collision >
<insert_const_block name="pr2_wrist_flex_collision"/>
</collision>
+ <map name="pr2_wrist_flex_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<link name="gripper_roll_left"><!-- link specifying the shoulder of the robot -->
@@ -1379,6 +1415,9 @@
<collision >
<insert_const_block name="pr2_gripper_roll_collision"/>
</collision>
+ <map name="pr2_gripper_roll_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<link name="gripper_roll_right"><!-- link specifying the shoulder of the robot -->
@@ -1397,6 +1436,9 @@
<collision >
<insert_const_block name="pr2_gripper_roll_collision"/>
</collision>
+ <map name="pr2_gripper_roll_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<!-- End arm definitions -->
@@ -1420,6 +1462,9 @@
<collision >
<insert_const_block name="pr2_finger_l_collision"/>
</collision>
+ <map name="finger_l_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<!-- End left hand l finger proximal digit definition -->
<!-- Begin left hand l finger tip (distal digit) definition -->
@@ -1439,6 +1484,9 @@
<collision >
<insert_const_block name="pr2_finger_tip_l_collision"/>
</collision>
+ <map name="finger_tip_l_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<!-- End left hand l finger tip (distal digit) definition -->
<!-- Begin left hand r finger proximal digit definition -->
@@ -1458,6 +1506,9 @@
<collision >
<insert_const_block name="pr2_finger_r_collision"/>
</collision>
+ <map name="finger_r_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<!-- End left hand r finger proximal digit definition -->
<!-- Begin left hand r finger tip (distal digit) definition -->
@@ -1477,6 +1528,9 @@
<collision >
<insert_const_block name="pr2_finger_tip_r_collision"/>
</collision>
+ <map name="finger_tip_r_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<!-- End left hand r finger tip (distal digit) definition -->
<!-- End left hand -->
@@ -1500,6 +1554,9 @@
<collision >
<insert_const_block name="pr2_finger_l_collision"/>
</collision>
+ <map name="finger_l_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<!-- End right hand l finger proximal digit definition -->
<!-- Begin right hand l finger tip (distal digit) definition -->
@@ -1519,6 +1576,9 @@
<collision >
<insert_const_block name="pr2_finger_tip_l_collision"/>
</collision>
+ <map name="finger_tip_l_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<!-- End right hand l finger tip (distal digit) definition -->
<!-- Begin right hand r finger proximal digit definition -->
@@ -1538,6 +1598,9 @@
<collision >
<insert_const_block name="pr2_finger_r_collision"/>
</collision>
+ <map name="finger_r_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<!-- End right hand r finger proximal digit definition -->
<!-- Begin right hand r finger tip (distal digit) definition -->
@@ -1557,6 +1620,9 @@
<collision >
<insert_const_block name="pr2_finger_tip_r_collision"/>
</collision>
+ <map name="finger_tip_r_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
</link>
<!-- End right hand r finger tip (distal digit) definition -->
<!-- End right hand -->
@@ -1770,6 +1836,9 @@
<!-- rotation of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
<joint name="wrist_camera_left_joint" />
+ <map name="wrist_camera_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
<inertial>
<mass value="0.5" />
<com xyz=" 0.0 0.0 0.0 " /> <!-- position of the center of mass with respect to the sensor's local coordinate frame -->
@@ -1830,6 +1899,9 @@
<!-- rotation of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
<joint name="wrist_camera_right_joint"/>
+ <map name="wrist_camera_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
<inertial>
<mass value="0.5" />
<com xyz=" 0.0 0.0 0.0 " /> <!-- position of the center of mass with respect to the sensor's local coordinate frame -->
@@ -1890,6 +1962,9 @@
<!-- rotation of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
<joint name="forearm_camera_left_joint" />
+ <map name="forearm_camera_left_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
<inertial>
<mass value="0.5" />
<com xyz=" 0.0 0.0 0.0 " /> <!-- position of the center of mass with respect to the sensor's local coordinate frame -->
@@ -1944,6 +2019,9 @@
<!-- rotation of a local coordinate frame attached to the sensor with respect to the parent link's coordinate frame -->
<joint name="forearm_camera_right_joint" />
+ <map name="forearm_camera_right_gravity" flag="gazebo">
+ <elem key="turnGravityOff">true</elem>
+ </map>
<inertial>
<mass value="0.5" />
<com xyz=" 0.0 0.0 0.0 " /> <!-- position of the center of mass with respect to the sensor's local coordinate frame -->
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-08-27 03:25:07
|
Revision: 3675
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3675&view=rev
Author: hsujohnhsu
Date: 2008-08-27 03:25:16 +0000 (Wed, 27 Aug 2008)
Log Message:
-----------
update P3D plugins to broadcast message with pose informations in robot_msgs/Pose3DEulerFloat32.
updated xmls to reflect change (added topic names and frame names).
Modified Paths:
--------------
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml
pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_gazebo_arm_test.xml
Added Paths:
-----------
pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg
pkg/trunk/robot_msgs/msg/Pose3DEulerFloat64.msg
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/P3D.hh 2008-08-27 03:25:16 UTC (rev 3675)
@@ -31,6 +31,9 @@
#include <gazebo/Entity.hh>
#include <pr2Core/pr2Core.h>
+#include <ros/node.h>
+#include <robot_msgs/Pose3DEulerFloat32.h>
+
namespace gazebo
{
class PositionIface;
@@ -84,6 +87,26 @@
private: Model *myParent;
private: Body *myBody; //Gazebo/ODE body
+
+
+
+ // added for ros message
+ // pointer to ros node
+ private: ros::node *rosnode;
+
+ // ros message
+ private: robot_msgs::Pose3DEulerFloat32 poseMsg;
+
+ // topic name
+ private: std::string topicName;
+
+ // frame transform name, should match link name
+ // FIXME: extract link name directly?
+ private: std::string frameName;
+
+ // A mutex to lock access to fields that are used in message callbacks
+ private: ros::thread::mutex lock;
+
};
/** \} */
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Camera.hh 2008-08-27 03:25:16 UTC (rev 3675)
@@ -27,9 +27,6 @@
#ifndef ROS_CAMERA_HH
#define ROS_CAMERA_HH
-#include <rosTF/rosTF.h>
-
-
#include <ros/node.h>
#include <std_msgs/Image.h>
#include <gazebo/Controller.hh>
@@ -118,9 +115,6 @@
// A mutex to lock access to fields that are used in message callbacks
private: ros::thread::mutex lock;
- // transform server
- private: rosTFClient *tfc;
-
};
/** /} */
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Ros_Laser.hh 2008-08-27 03:25:16 UTC (rev 3675)
@@ -32,7 +32,6 @@
#include <ros/node.h>
#include <std_msgs/LaserScan.h>
-#include <rosTF/rosTF.h>
namespace gazebo
{
@@ -128,9 +127,6 @@
// A mutex to lock access to fields that are used in message callbacks
private: ros::thread::mutex lock;
- // transform server
- private: rosTFClient *tfc;
-
};
/** /} */
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/manifest.xml 2008-08-27 03:25:16 UTC (rev 3675)
@@ -20,7 +20,7 @@
<depend package="rosTF"/>
<depend package="libTF"/>
<depend package="rosthread"/>
-<depend package="std_msgs"/>
+<depend package="robot_msgs"/>
<depend package="stl_utils" />
<depend package="math_utils" />
<depend package="gazebo_robot_description"/>
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/src/P3D.cc 2008-08-27 03:25:16 UTC (rev 3675)
@@ -51,6 +51,17 @@
if (!this->myParent)
gzthrow("P3D controller requires a Model as its parent");
+
+ rosnode = ros::g_node; // comes from where?
+ int argc = 0;
+ char** argv = NULL;
+ if (rosnode == NULL)
+ {
+ // this only works for a single camera.
+ ros::init(argc,argv);
+ rosnode = new ros::node("ros_gazebo",ros::node::DONT_HANDLE_SIGINT);
+ printf("-------------------- starting node in P3D \n");
+ }
}
////////////////////////////////////////////////////////////////////////////////
@@ -71,6 +82,12 @@
std::string bodyName = node->GetString("bodyName", "", 1);
this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
// this->myBody = dynamic_cast<Body*>(this->myParent->GetBody(bodyName));
+
+ this->topicName = node->GetString("topicName", "", 1);
+ this->frameName = node->GetString("frameName", "", 1);
+
+ std::cout << "==== topic name for P3D ======== " << this->topicName << std::endl;
+ rosnode->advertise<robot_msgs::Pose3DEulerFloat32>(this->topicName);
}
////////////////////////////////////////////////////////////////////////////////
@@ -99,6 +116,29 @@
this->myIface->data->pose.pitch = rot.GetPitch();
this->myIface->data->pose.yaw = rot.GetYaw();
+
+
+ this->lock.lock();
+ // copy data into pose message
+ this->poseMsg.header.frame_id = this->frameName;
+ this->poseMsg.header.stamp.sec = (unsigned long)floor(this->myIface->data->head.time);
+ this->poseMsg.header.stamp.nsec = (unsigned long)floor( 1e9 * ( this->myIface->data->head.time - this->poseMsg.header.stamp.sec) );
+
+ this->poseMsg.position.x = pos.x;
+ this->poseMsg.position.y = pos.y;
+ this->poseMsg.position.z = pos.z;
+
+ this->poseMsg.orientation.roll = rot.GetRoll();
+ this->poseMsg.orientation.pitch = rot.GetPitch();
+ this->poseMsg.orientation.yaw = rot.GetYaw();
+
+ // publish to ros
+ rosnode->publish(this->topicName,this->poseMsg);
+ this->lock.unlock();
+
+
+
+
this->myIface->Unlock();
}
@@ -106,4 +146,12 @@
// Finalize the controller
void P3D::FiniChild()
{
+ // TODO: will be replaced by global ros node eventually
+ if (rosnode != NULL)
+ {
+ std::cout << "shutdown rosnode in P3D" << std::endl;
+ //ros::fini();
+ rosnode->shutdown();
+ //delete rosnode;
+ }
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo.xml 2008-08-27 03:25:16 UTC (rev 3675)
@@ -42,18 +42,24 @@
<controller:P3D name="p3d_right_wrist_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
+ <topicName>gripper_roll_right_pose</topicName>
+ <frameName>gripper_roll_right_frame</frameName>
<interface:position name="p3d_right_wrist_position"/>
</controller:P3D>
<controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
+ <topicName>gripper_roll_left_pose</topicName>
+ <frameName>gripper_roll_left_frame</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
<controller:P3D name="p3d_base_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>base</bodyName>
+ <topicName>base_pose</topicName>
+ <frameName>base_frame</frameName>
<interface:position name="p3d_base_position"/>
</controller:P3D>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/controllers_gazebo_test.xml 2008-08-27 03:25:16 UTC (rev 3675)
@@ -48,18 +48,24 @@
<controller:P3D name="p3d_right_wrist_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>gripper_roll_right</bodyName>
+ <topicName>gripper_roll_right_pose</topicName>
+ <frameName>gripper_roll_right_frame</frameName>
<interface:position name="p3d_right_wrist_position"/>
</controller:P3D>
<controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
+ <topicName>gripper_roll_left_pose</topicName>
+ <frameName>gripper_roll_left_frame</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
<controller:P3D name="p3d_base_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>base</bodyName>
+ <topicName>base_pose</topicName>
+ <frameName>base_frame</frameName>
<interface:position name="p3d_base_position"/>
</controller:P3D>
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_gazebo_arm_test.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_gazebo_arm_test.xml 2008-08-27 03:11:47 UTC (rev 3674)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2_arm_test/controllers_gazebo_arm_test.xml 2008-08-27 03:25:16 UTC (rev 3675)
@@ -33,6 +33,8 @@
<controller:P3D name="p3d_left_wrist_controller" plugin="libP3D.so">
<updateRate>100.0</updateRate>
<bodyName>gripper_roll_left</bodyName>
+ <topicName>gripper_roll_left_pose</topicName>
+ <frameName>gripper_roll_left_frame</frameName>
<interface:position name="p3d_left_wrist_position"/>
</controller:P3D>
Added: pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/Pose3DEulerFloat32.msg 2008-08-27 03:25:16 UTC (rev 3675)
@@ -0,0 +1,3 @@
+Header header
+std_msgs/Point3DFloat32 position
+std_msgs/EulerAnglesFloat32 orientation
Added: pkg/trunk/robot_msgs/msg/Pose3DEulerFloat64.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/Pose3DEulerFloat64.msg (rev 0)
+++ pkg/trunk/robot_msgs/msg/Pose3DEulerFloat64.msg 2008-08-27 03:25:16 UTC (rev 3675)
@@ -0,0 +1,3 @@
+Header header
+std_msgs/Point3DFloat64 position
+std_msgs/EulerAnglesFloat64 orientation
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-27 03:55:28
|
Revision: 3677
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3677&view=rev
Author: isucan
Date: 2008-08-27 03:55:37 +0000 (Wed, 27 Aug 2008)
Log Message:
-----------
fixed bug in world_3d_map (correct order of initialization), added more things in the collision group, removed extra messages in collision space
Modified Paths:
--------------
pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml
pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml
pkg/trunk/world_models/world_3d_map/run.xml
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml 2008-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo-fake_localization.xml 2008-08-27 03:55:37 UTC (rev 3677)
@@ -1,13 +1,11 @@
<launch>
<group name="wg">
- <include file="$(find wg_robot_description)/send.xml"/>
- <node pkg="gazebo" type="run-gazebo.sh" args="$(find gazebo_robot_description)/world/robot_rosgazebo.world" respawn="true" />
- <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" />
- <node pkg="pr2_gazebo" type="run-pr2_gazebo.sh" args="" respawn="true" />
- <node pkg="fake_localization" type="fake_localization" respawn="false" />
- <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" />
- <node pkg="nav_view" type="nav_view" respawn="true" />
- <node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" />
+ <include file="$(find gazebo_robot_description)/pr2_test.xml"/>
+ <node pkg="map_server" type="map_server" args="$(find gazebo_robot_description)/world/Media/materials/textures/map3.png 0.1" respawn="false" output="screen" />
+ <node pkg="fake_localization" type="fake_localization" respawn="false" output="screen" />
+ <node pkg="wavefront_player" type="wavefront_player" args="scan:=base_scan" respawn="false" output="screen" />
+ <node pkg="nav_view" type="nav_view" respawn="false" output="screen" />
+ <node pkg="teleop_base_keyboard" type="teleop_base_keyboard" respawn="false" output="screen" />
</group>
</launch>
Modified: pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp
===================================================================
--- pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/motion_planning/collision_space/src/collision_space/environmentODE.cpp 2008-08-27 03:55:37 UTC (rev 3677)
@@ -250,16 +250,14 @@
dReal aabb1[6], aabb2[6];
dGeomGetAABB(g1, aabb1);
dGeomGetAABB(g2, aabb2);
+
if (!(aabb1[2] > aabb2[3] ||
aabb1[3] < aabb2[2] ||
aabb1[4] > aabb2[5] ||
- aabb1[5] < aabb2[4]))
+ aabb1[5] < aabb2[4]))
dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
if (cdata.collides)
- {
- std::cout << m_kgeoms[model_id].geom[vec[j]]->link->name << " intersects with " << m_kgeoms[model_id].geom[vec[k]]->link->name << std::endl;
goto OUT1;
- }
}
}
}
@@ -280,11 +278,13 @@
dGeomID g2 = m_odeGeoms[j];
dReal aabb2[6];
dGeomGetAABB(g2, aabb2);
+
if (!(aabb1[2] > aabb2[3] ||
aabb1[3] < aabb2[2] ||
aabb1[4] > aabb2[5] ||
aabb1[5] < aabb2[4]))
dSpaceCollide2(g1, g2, reinterpret_cast<void*>(&cdata), nearCallbackFn);
+
if (cdata.collides)
goto OUT2;
}
@@ -299,8 +299,6 @@
m_collide2.setup();
for (int i = m_kgeoms[model_id].geom.size() - 1 ; i >= 0 && !cdata.collides ; --i)
m_collide2.collide(m_kgeoms[model_id].geom[i]->geom, reinterpret_cast<void*>(&cdata), nearCallbackFn);
- if (cdata.collides)
- std::cout << "Pointcloud intersection" << std::endl;
}
return cdata.collides;
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-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2008-08-27 03:55:37 UTC (rev 3677)
@@ -78,6 +78,7 @@
req.params.model_id = "pr2::base";
req.params.distance_metric = "L2Square";
+ req.params.planner_id = "RRT";
req.threshold = 0.01;
req.interpolate = 1;
req.times = 10;
@@ -89,7 +90,7 @@
req.goal_state.vals[i] = m_basePos[i];
req.goal_state.vals[0] += 3.5;
- req.allowed_time = 30.0;
+ req.allowed_time = 10.0;
req.params.volumeMin.x = -5.0 + m_basePos[0]; req.params.volumeMin.y = -5.0 + m_basePos[1]; req.params.volumeMin.z = 0.0;
req.params.volumeMax.x = 5.0 + m_basePos[0]; req.params.volumeMax.y = 5.0 + m_basePos[1]; req.params.volumeMax.z = 0.0;
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-08-27 03:55:37 UTC (rev 3677)
@@ -96,6 +96,9 @@
m_node = node;
m_basePos[0] = m_basePos[1] = m_basePos[2] = 0.0;
m_robotModelName = robot_model;
+ m_haveState = false;
+ m_haveMechanismState = false;
+ m_haveBasePos = false;
m_node->subscribe("localizedpose", m_localizedPose, &NodeRobotModel::localizedPoseCallback, this, 1);
m_node->subscribe("mechanism_state", m_mechanismState, &NodeRobotModel::mechanismStateCallback, this, 1);
@@ -174,6 +177,12 @@
return m_kmodel != NULL;
}
+ void waitForState(void)
+ {
+ while (m_node->ok() && !(m_haveState ^ loadedRobot()))
+ ros::Duration(0.05).sleep();
+ }
+
protected:
void localizedPoseCallback(void)
@@ -214,7 +223,7 @@
m_basePos[1] = pose.y;
if (isfinite(pose.yaw))
m_basePos[2] = pose.yaw;
-
+ m_haveBasePos = true;
baseUpdate();
}
}
@@ -227,6 +236,7 @@
double pos = m_mechanismState.joint_states[i].position;
m_robotState->setValue(m_mechanismState.joint_states[i].name, &pos);
}
+ m_haveMechanismState = true;
stateUpdate();
}
@@ -243,13 +253,16 @@
virtual void stateUpdate(void)
{
+ m_haveState = m_haveBasePos && m_haveMechanismState;
}
rosTFClient m_tf;
ros::node *m_node;
std_msgs::RobotBase2DOdom m_localizedPose;
+ bool m_haveBasePos;
mechanism_control::MechanismState m_mechanismState; // this message should be moved to robot_msgs
+ bool m_haveMechanismState;
robot_desc::URDF *m_urdf;
planning_models::KinematicModel *m_kmodel;
@@ -258,7 +271,8 @@
double m_basePos[3];
planning_models::KinematicModel::StateParams *m_robotState;
-
+ bool m_haveState;
+
};
}
Modified: pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml 2008-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/robot_descriptions/wg_robot_description/pr2/groups_collision.xml 2008-08-27 03:55:37 UTC (rev 3677)
@@ -19,6 +19,12 @@
finger_r_left
finger_l_right
finger_r_right
+ shoulder_pan_right
+ shoulder_pitch_right
+ shoulder_pan_left
+ shoulder_pitch_left
+ torso
+ base
</group>
<!-- Define the parts of the robot that the collision checker should care about -->
Modified: pkg/trunk/world_models/world_3d_map/run.xml
===================================================================
--- pkg/trunk/world_models/world_3d_map/run.xml 2008-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/world_models/world_3d_map/run.xml 2008-08-27 03:55:37 UTC (rev 3677)
@@ -2,8 +2,8 @@
<param name="world_3d_map/max_publish_frequency" type="double" value="0.3" /> <!-- Hz -->
<param name="world_3d_map/retain_pointcloud_duration" type="double" value="60.0" /> <!-- seconds -->
- <param name="world_3d_map/retain_pointcloud_fraction" type="double" value="0.05" /> <!-- percentage (between 0 and 1) -->
- <param name="world_3d_map/retain_above_ground_threshold" type="double" value="0.02" /> <!-- double value -->
+ <param name="world_3d_map/retain_pointcloud_fraction" type="double" value="0.25" /> <!-- percentage (between 0 and 1) -->
+ <param name="world_3d_map/retain_above_ground_threshold" type="double" value="0.03" /> <!-- double value -->
<param name="world_3d_map/verbosity_level" type="int" value="1" /> <!-- integer value -->
<node pkg="world_3d_map" type="world_3d_map" args="robotdesc/pr2 scan:=tilt_scan" output="screen" respawn="false" />
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-27 03:48:14 UTC (rev 3676)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-27 03:55:37 UTC (rev 3677)
@@ -125,7 +125,8 @@
param("world_3d_map/max_publish_frequency", m_maxPublishFrequency, 0.5);
param("world_3d_map/retain_pointcloud_duration", m_retainPointcloudDuration, 60.0);
- param("world_3d_map/retain_pointcloud_fraction", m_retainPointcloudFraction, 0.02);
+ param("world_3d_map/retain_pointcloud_fraction", m_retainPointcloudFraction, 0.25);
+
param("world_3d_map/retain_above_ground_threshold", m_retainAboveGroundThreshold, 0.01);
param("world_3d_map/verbosity_level", m_verbose, 1);
@@ -191,7 +192,7 @@
postpone processing latest data just because it is not done
with older data. */
if (m_verbose)
- fprintf(stdout, "Received laser scan with %d points in frame %s\n", m_inputScan.get_ranges_size(), m_inputScan.header.frame_id.c_str());
+ fprintf(stdout, "Received laser scan with %d points in frame %s\n", m_inputScan.get_ranges_size(), m_inputScan.header.frame_id.c_str());
/* copy data to a place where incoming messages do not affect it */
bool success = false;
@@ -388,7 +389,9 @@
if (isfinite(cloud.pts[k].x) && isfinite(cloud.pts[k].y) && isfinite(cloud.pts[k].z))
copy->pts[j++] = cloud.pts[k];
copy->set_pts_size(j);
-
+
+ if (m_verbose)
+ printf("Filter 0 discarded %d points (%d left) \n", n - j, j);
return copy;
}
@@ -420,7 +423,7 @@
}
}
if (m_verbose)
- printf("Discarded %d points (%d left) \n", n - j, j);
+ printf("Filter 1 discarded %d points (%d left) \n", n - j, j);
copy->set_pts_size(j);
@@ -463,6 +466,7 @@
{
World3DMap *map = new World3DMap(argv[1]);
map->loadRobotDescription();
+ map->waitForState();
map->setAcceptScans(true);
map->spin();
map->shutdown();
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <is...@us...> - 2008-08-27 20:17:39
|
Revision: 3697
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3697&view=rev
Author: isucan
Date: 2008-08-27 20:17:48 +0000 (Wed, 27 Aug 2008)
Log Message:
-----------
little updates to allow more extensible processing of data in world_3d_map
Modified Paths:
--------------
pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
Modified: pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h
===================================================================
--- pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-08-27 20:14:51 UTC (rev 3696)
+++ pkg/trunk/motion_planning/planning_node_util/include/planning_node_util/knode.h 2008-08-27 20:17:48 UTC (rev 3697)
@@ -179,7 +179,7 @@
void waitForState(void)
{
- while (m_node->ok() && !(m_haveState ^ loadedRobot()))
+ while (m_node->ok() && (m_haveState ^ loadedRobot()))
ros::Duration(0.05).sleep();
}
Modified: pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp
===================================================================
--- pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-27 20:14:51 UTC (rev 3696)
+++ pkg/trunk/motion_planning/planning_world_viewer/src/planning_world_viewer.cpp 2008-08-27 20:17:48 UTC (rev 3697)
@@ -353,21 +353,17 @@
viewer->loadRobotDescription();
viewer->updateODESpaces();
- if (viewer->loadedRobot())
- {
- dsFunctions fn;
- fn.version = DS_VERSION;
- fn.start = &start;
- fn.step = &simLoop;
- fn.command = &command;
- fn.stop = 0;
- fn.path_to_textures = "./res";
-
- dsSimulationLoop(argc, argv, 640, 480, &fn);
- }
- else
- printf("No model defined. Display world node cannot start.\n");
+ dsFunctions fn;
+ fn.version = DS_VERSION;
+ fn.start = &start;
+ fn.step = &simLoop;
+ fn.command = &command;
+ fn.stop = 0;
+ fn.path_to_textures = "./res";
+
+ dsSimulationLoop(argc, argv, 640, 480, &fn);
+
viewer->shutdown();
delete viewer;
Modified: pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp
===================================================================
--- pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-27 20:14:51 UTC (rev 3696)
+++ pkg/trunk/world_models/world_3d_map/src/world_3d_map.cpp 2008-08-27 20:17:48 UTC (rev 3697)
@@ -75,6 +75,7 @@
Subscribes to (name/type):
- @b scan/LaserScan : scan data received from a laser
+- @b cloud/PointCloudFloat32 : cloud data
Additional subscriptions due to inheritance from NodeRobotModel:
- @b localizedpose/RobotBase2DOdom : localized position of the robot base
@@ -138,7 +139,8 @@
/* create a thread that handles the publishing of the data */
m_publishingThread = ros::thread::member_thread::startMemberFunctionThread<World3DMap>(this, &World3DMap::publishDataThread);
- subscribe("scan", m_inputScan, &World3DMap::pointCloudCallback, 1);
+ subscribe("scan", m_inputScan, &World3DMap::scanCallback, 1);
+ subscribe("cloud", m_inputCloud, &World3DMap::pointCloudCallback, 1);
}
~World3DMap(void)
@@ -163,6 +165,8 @@
void setAcceptScans(bool acceptScans)
{
m_acceptScans = acceptScans;
+ if (m_verbose)
+ printf("Accepting input scans...\n");
}
private:
@@ -185,12 +189,48 @@
/* If we're not ready to accept scans yet, discard the data */
if (!m_acceptScans)
return;
+
+ if (m_verbose)
+ fprintf(stdout, "Received point cloud with %d points in frame %s\n", m_inputCloud.get_pts_size(), m_inputCloud.header.frame_id.c_str());
- /* The idea is that if processing of previous input data is
- not done, data will be discarded. Hopefully this discarding
- of data will not happen, but we don't want the node to
- postpone processing latest data just because it is not done
- with older data. */
+ bool success = false;
+
+ if (m_inputCloud.header.frame_id == "FRAMEID_MAP")
+ {
+ m_toProcess = m_inputCloud;
+ success = true;
+ }
+ else
+ {
+ try
+ {
+ m_tf.transformPointCloud("FRAMEID_MAP", m_toProcess, m_inputCloud);
+ success = true;
+ }
+ catch(libTF::TransformReference::LookupException& ex)
+ {
+ fprintf(stderr, "Discarding pointcloud: Transform reference lookup exception\n");
+ }
+ catch(libTF::TransformReference::ExtrapolateException& ex)
+ {
+ fprintf(stderr, "Discarding pointcloud: Extrapolation exception: %s\n", ex.what());
+ }
+ catch(...)
+ {
+ fprintf(stderr, "Discarding pointcloud: Exception in point cloud computation\n");
+ }
+ }
+
+ if (success)
+ processData();
+ }
+
+ void scanCallback(void)
+ {
+ /* If we're not ready to accept scans yet, discard the data */
+ if (!m_acceptScans)
+ return;
+
if (m_verbose)
fprintf(stdout, "Received laser scan with %d points in frame %s\n", m_inputScan.get_ranges_size(), m_inputScan.header.frame_id.c_str());
@@ -203,15 +243,15 @@
}
catch(libTF::TransformReference::LookupException& ex)
{
- fprintf(stderr, "Discarding pointcloud: Transform reference lookup exception\n");
+ fprintf(stderr, "Discarding laser scan: Transform reference lookup exception\n");
}
catch(libTF::TransformReference::ExtrapolateException& ex)
{
- fprintf(stderr, "Discarding pointcloud: Extrapolation exception: %s\n", ex.what());
+ fprintf(stderr, "Discarding laser scan: Extrapolation exception: %s\n", ex.what());
}
catch(...)
{
- fprintf(stderr, "Discarding pointcloud: Exception in point cloud computation\n");
+ fprintf(stderr, "Discarding laser scan: Exception in point cloud computation\n");
}
if (success)
processData();
@@ -440,7 +480,8 @@
double m_retainAboveGroundThreshold;
int m_verbose;
- std_msgs::LaserScan m_inputScan; //Buffer for recieving scan
+ std_msgs::LaserScan m_inputScan; //Buffer for recieving scan
+ std_msgs::PointCloudFloat32 m_inputCloud; //Buffer for recieving cloud
std_msgs::PointCloudFloat32 m_toProcess;
pthread_t *m_publishingThread;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <hsu...@us...> - 2008-08-27 23:32:24
|
Revision: 3723
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3723&view=rev
Author: hsujohnhsu
Date: 2008-08-27 23:32:33 +0000 (Wed, 27 Aug 2008)
Log Message:
-----------
added option to use quick step in ode with a tag <quickStep>true</quickStep>.
Modified Paths:
--------------
pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world
pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test.world
Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-08-27 23:13:41 UTC (rev 3722)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff 2008-08-27 23:32:33 UTC (rev 3723)
@@ -809,11 +809,68 @@
}
+Index: server/physics/ode/ODEPhysics.hh
+===================================================================
+--- server/physics/ode/ODEPhysics.hh (revision 6981)
++++ server/physics/ode/ODEPhysics.hh (working copy)
+@@ -133,6 +133,7 @@
+
+ private: Param<double> *globalCFMP;
+ private: Param<double> *globalERPP;
++ private: Param<bool> *quickStepP;
+ };
+
+ /** \}*/
Index: server/physics/ode/ODEPhysics.cc
===================================================================
--- server/physics/ode/ODEPhysics.cc (revision 6981)
+++ server/physics/ode/ODEPhysics.cc (working copy)
-@@ -264,15 +264,16 @@
+@@ -69,6 +69,7 @@
+
+ this->globalCFMP = new Param<double>("cfm", 10e-5, 0);
+ this->globalERPP = new Param<double>("erp", 0.2, 0);
++ this->quickStepP = new Param<bool>("quickStep", false, 0);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -86,6 +87,7 @@
+
+ delete this->globalCFMP;
+ delete this->globalERPP;
++ delete this->quickStepP;
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -101,6 +103,7 @@
+ this->updateRateP->Load(cnode);
+ this->globalCFMP->Load(cnode);
+ this->globalERPP->Load(cnode);
++ this->quickStepP->Load(cnode);
+ }
+
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -113,6 +116,7 @@
+ stream << prefix << " " << *(this->updateRateP) << "\n";
+ stream << prefix << " " << *(this->globalCFMP) << "\n";
+ stream << prefix << " " << *(this->globalERPP) << "\n";
++ stream << prefix << " " << *(this->quickStepP) << "\n";
+ stream << prefix << "</physics:ode>\n";
+ }
+
+@@ -135,8 +139,10 @@
+ dSpaceCollide( this->spaceId, this, CollisionCallback );
+
+ // Update the dynamical model
+- dWorldStep( this->worldId, this->stepTimeP->GetValue() );
+- //dWorldQuickStep(this->worldId, this->stepTime);
++ if (this->quickStepP->GetValue())
++ dWorldQuickStep(this->worldId, this->stepTimeP->GetValue() );
++ else
++ dWorldStep( this->worldId, this->stepTimeP->GetValue() );
+
+ // Very important to clear out the contact group
+ dJointGroupEmpty( this->contactGroup );
+@@ -264,15 +270,16 @@
contact.surface.mode = dContactSlip1 | dContactSlip2 |
dContactSoftERP | dContactSoftCFM |
dContactBounce | dContactMu2 | dContactApprox1;
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-08-27 23:13:41 UTC (rev 3722)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot.world 2008-08-27 23:32:33 UTC (rev 3723)
@@ -28,6 +28,7 @@
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
+ <quickStep>false</quickStep>
</physics:ode>
<geo:origin>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world 2008-08-27 23:13:41 UTC (rev 3722)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_arm_test.world 2008-08-27 23:32:33 UTC (rev 3723)
@@ -28,6 +28,7 @@
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
+ <quickStep>false</quickStep>
</physics:ode>
<geo:origin>
Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test.world 2008-08-27 23:13:41 UTC (rev 3722)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/world/robot_test.world 2008-08-27 23:32:33 UTC (rev 3723)
@@ -28,6 +28,7 @@
<gravity>0 0 -9.8</gravity>
<cfm>0.0000001</cfm>
<erp>0.2</erp>
+ <quickStep>false</quickStep>
</physics:ode>
<geo:origin>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <ge...@us...> - 2008-08-28 18:16:01
|
Revision: 3751
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3751&view=rev
Author: gerkey
Date: 2008-08-28 18:16:09 +0000 (Thu, 28 Aug 2008)
Log Message:
-----------
Removed gtest from manifests.
Modified Paths:
--------------
pkg/trunk/motion_planning/collision_space/manifest.xml
pkg/trunk/motion_planning/kinematic_planning/manifest.xml
pkg/trunk/motion_planning/sbpl/manifest.xml
pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml
pkg/trunk/util/laser_scan_utils/manifest.xml
pkg/trunk/util/libTF/manifest.xml
pkg/trunk/util/math_utils/manifest.xml
pkg/trunk/util/string_utils/manifest.xml
pkg/trunk/visualization/ogre_tools/manifest.xml
pkg/trunk/world_models/map_server/CMakeLists.txt
pkg/trunk/world_models/map_server/manifest.xml
Modified: pkg/trunk/motion_planning/collision_space/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/collision_space/manifest.xml 2008-08-28 17:35:27 UTC (rev 3750)
+++ pkg/trunk/motion_planning/collision_space/manifest.xml 2008-08-28 18:16:09 UTC (rev 3751)
@@ -10,7 +10,6 @@
<depend package="planning_models"/>
<depend package="scan_utils"/>
<depend package="opende"/>
- <depend package="gtest"/>
<depend package="profiling_utils"/>
<export>
Modified: pkg/trunk/motion_planning/kinematic_planning/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-08-28 17:35:27 UTC (rev 3750)
+++ pkg/trunk/motion_planning/kinematic_planning/manifest.xml 2008-08-28 18:16:09 UTC (rev 3751)
@@ -9,5 +9,4 @@
<depend package="ompl" />
<depend package="profiling_utils" />
<depend package="string_utils" />
- <depend package="gtest" />
</package>
Modified: pkg/trunk/motion_planning/sbpl/manifest.xml
===================================================================
--- pkg/trunk/motion_planning/sbpl/manifest.xml 2008-08-28 17:35:27 UTC (rev 3750)
+++ pkg/trunk/motion_planning/sbpl/manifest.xml 2008-08-28 18:16:09 UTC (rev 3751)
@@ -11,5 +11,4 @@
<cpp cflags="-I${prefix}/src -O3" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lsbpl"/>
</export>
- <depend package="gtest"/>
</package>
Modified: pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml
===================================================================
--- pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml 2008-08-28 17:35:27 UTC (rev 3750)
+++ pkg/trunk/robot_descriptions/wg_robot_description_parser/manifest.xml 2008-08-28 18:16:09 UTC (rev 3751)
@@ -13,7 +13,6 @@
<depend package="math_utils"/>
<depend package="string_utils"/>
<depend package="stl_utils" />
- <depend package="gtest"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lURDF"/>
Modified: pkg/trunk/util/laser_scan_utils/manifest.xml
===================================================================
--- pkg/trunk/util/laser_scan_utils/manifest.xml 2008-08-28 17:35:27 UTC (rev 3750)
+++ pkg/trunk/util/laser_scan_utils/manifest.xml 2008-08-28 18:16:09 UTC (rev 3751)
@@ -10,7 +10,6 @@
<depend package="newmat10"/>
<depend package="std_msgs"/>
<depend package="roscpp"/>
-<depend package="gtest"/>
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -llaser_scan"/>
</export>
Modified: pkg/trunk/util/libTF/manifest.xml
===================================================================
--- pkg/trunk/util/libTF/manifest.xml 2008-08-28 17:35:27 UTC (rev 3750)
+++ pkg/trunk/util/libTF/manifest.xml 2008-08-28 18:16:09 UTC (rev 3751)
@@ -8,7 +8,6 @@
<license>BSD</license>
<url>http://pr.willowgarage.com</url>
<depend package="newmat10"/>
-<depend package="gtest"/>
<depend package="ros_exception"/>
<depend package="rosthread"/>
<depend package="math_utils"/>
Modified: pkg/trunk/util/math_utils/manifest.xml
===================================================================
--- pkg/trunk/util/math_utils/manifest.xml 2008-08-28 17:35:27 UTC (rev 3750)
+++ pkg/trunk/util/math_utils/manifest.xml 2008-08-28 18:16:09 UTC (rev 3751)
@@ -6,7 +6,6 @@
</description>
<author>John Hsu, Ioan Sucan</author>
<license>BSD</license>
- <depend package="gtest" />
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lmath_utils"/>
</export>
Modified: pkg/trunk/util/string_utils/manifest.xml
===================================================================
--- pkg/trunk/util/string_utils/manifest.xml 2008-08-28 17:35:27 UTC (rev 3750)
+++ pkg/trunk/util/string_utils/manifest.xml 2008-08-28 18:16:09 UTC (rev 3751)
@@ -11,8 +11,6 @@
<author>Ioan Sucan, Morgan Quigley (email: mqu...@cs...)</author>
<license>BSD</license>
- <depend package="gtest" />
-
<export>
<cpp cflags="-I${prefix}/include" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lstring_utils"/>
</export>
Modified: pkg/trunk/visualization/ogre_tools/manifest.xml
===================================================================
--- pkg/trunk/visualization/ogre_tools/manifest.xml 2008-08-28 17:35:27 UTC (rev 3750)
+++ pkg/trunk/visualization/ogre_tools/manifest.xml 2008-08-28 18:16:09 UTC (rev 3751)
@@ -8,7 +8,6 @@
<license>BSD</license>
<url>http://pr.willowgarage.com/wiki/ROS</url>
<depend package="ogre"/>
- <depend package="gtest"/>
<export>
<cpp cflags="-I${prefix}/src/" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -logretools"/>
</export>
Modified: pkg/trunk/world_models/map_server/CMakeLists.txt
===================================================================
--- pkg/trunk/world_models/map_server/CMakeLists.txt 2008-08-28 17:35:27 UTC (rev 3750)
+++ pkg/trunk/world_models/map_server/CMakeLists.txt 2008-08-28 18:16:09 UTC (rev 3751)
@@ -7,6 +7,7 @@
RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
rospack_add_executable(rtest test/rtest.cpp test/test_constants.cpp)
+rospack_add_gtest_build_flags(rtest)
set_target_properties(rtest PROPERTIES
RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/test)
Modified: pkg/trunk/world_models/map_server/manifest.xml
===================================================================
--- pkg/trunk/world_models/map_server/manifest.xml 2008-08-28 17:35:27 UTC (rev 3750)
+++ pkg/trunk/world_models/map_server/manifest.xml 2008-08-28 18:16:09 UTC (rev 3751)
@@ -11,5 +11,4 @@
<depend package="roscpp"/>
<depend package="std_srvs"/>
<depend package="sdl_image"/>
- <depend package="gtest"/>
</package>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-08-28 23:14:48
|
Revision: 3772
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3772&view=rev
Author: tfoote
Date: 2008-08-28 23:14:57 +0000 (Thu, 28 Aug 2008)
Log Message:
-----------
fixed dependants on axis_cam, bug in pr2_gui (thanks Josh), include and lib path on axis cam
Modified Paths:
--------------
pkg/trunk/drivers/cam/axis_cam/manifest.xml
pkg/trunk/visualization/pr2_gui/manifest.xml
pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp
pkg/trunk/visualization/pr2_gui/src/launcherimpl.h
pkg/trunk/visualization/wx_camera_panel/manifest.xml
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp
pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.h
Modified: pkg/trunk/drivers/cam/axis_cam/manifest.xml
===================================================================
--- pkg/trunk/drivers/cam/axis_cam/manifest.xml 2008-08-28 23:01:08 UTC (rev 3771)
+++ pkg/trunk/drivers/cam/axis_cam/manifest.xml 2008-08-28 23:14:57 UTC (rev 3772)
@@ -21,7 +21,7 @@
<sysdepend package="libcurl3-openssl-dev" os="ubuntu" version="7.04-feisty"/>
<sysdepend package="libcurl4-openssl-dev" os="ubuntu" version="8.04-hardy"/>
<export>
- <cpp cflags="-I${prefix}/include" lflags="-L${prefix}/lib -laxis_cam -lcurl"/>
+ <cpp cflags="-I${prefix}/include -I${prefix}/msg/cpp" lflags="-L${prefix}/lib -laxis -lcurl"/>
</export>
</package>
Modified: pkg/trunk/visualization/pr2_gui/manifest.xml
===================================================================
--- pkg/trunk/visualization/pr2_gui/manifest.xml 2008-08-28 23:01:08 UTC (rev 3771)
+++ pkg/trunk/visualization/pr2_gui/manifest.xml 2008-08-28 23:14:57 UTC (rev 3772)
@@ -13,6 +13,7 @@
<depend package="std_msgs"/>
<depend package="irrlicht_viewer"/>
<depend package="pr2Core"/>
+<depend package="axis_cam"/>
<depend package="rosTF"/>
<depend package="wg_robot_description_parser"/>
<export>
Modified: pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp 2008-08-28 23:01:08 UTC (rev 3771)
+++ pkg/trunk/visualization/pr2_gui/src/launcherimpl.cpp 2008-08-28 23:14:57 UTC (rev 3772)
@@ -38,8 +38,7 @@
{
//std::cerr << "errorOut!!!\n";
long first = Ros_TC->GetLastPosition();
- wxString line;
- line.FromUTF8(rosErrMsg.msg.c_str());
+ wxString line = wxString::FromAscii( rosErrMsg.msg.c_str() );
Ros_TC->AppendText(line);
}
@@ -261,7 +260,7 @@
PTZL_B->Enable(true);
myNode->subscribe("image_ptz_left", PTZLImage, &LauncherImpl::incomingPTZLImageConn,this);
myNode->subscribe("PTZL_state", PTZL_state, &LauncherImpl::incomingPTZLState,this);
- myNode->advertise<std_msgs::PTZActuatorCmd>("PTZL_cmd");
+ myNode->advertise<axis_cam::PTZActuatorCmd>("PTZL_cmd");
}
else
{
@@ -363,7 +362,7 @@
PTZR_B->Enable(true);
myNode->subscribe("image_ptz_right", PTZRImage, &LauncherImpl::incomingPTZRImageConn,this);
myNode->subscribe("PTZR_state", PTZR_state, &LauncherImpl::incomingPTZRState,this);
- myNode->advertise<std_msgs::PTZActuatorCmd>("PTZR_cmd");
+ myNode->advertise<axis_cam::PTZActuatorCmd>("PTZR_cmd");
}
else
{
Modified: pkg/trunk/visualization/pr2_gui/src/launcherimpl.h
===================================================================
--- pkg/trunk/visualization/pr2_gui/src/launcherimpl.h 2008-08-28 23:01:08 UTC (rev 3771)
+++ pkg/trunk/visualization/pr2_gui/src/launcherimpl.h 2008-08-28 23:14:57 UTC (rev 3772)
@@ -58,8 +58,8 @@
- @b "PTZR_image"/std_msgs::Image : Image received from the right PTZ
- @b "WristL_image"/std_msgs::Image : Image received from the left wrist camera
- @b "WristR_image"/std_msgs::Image : Image received from the right wrist camera
-- @b "PTZL_state"/std_msgs::PTZActuatorState : Receives state from the left PTZ
-- @b "PTZR_state"/std_msgs::PTZActuatorState : Receives state from the right PTZ
+- @b "PTZL_state"/axis_cam::PTZActuatorState : Receives state from the left PTZ
+- @b "PTZR_state"/axis_cam::PTZActuatorState : Receives state from the right PTZ
- @b "/roserr"/rostools::log : Receives roserr messages
- @b "cloud"/std_msgs::PointCloudFloat32 : Point cloud received from head Hokuyo
- @b "scan"/std_msgs::LaserScan : Laser cloud received from base Hokuyo
@@ -71,8 +71,8 @@
- @b "transform"/std_msgs::Empty : Cue to update model (new transform is available)
Publishes to (name/type):
-- @b "PTZR_cmd"/std_msgs::std_msgs::PTZActuatorCmd : Moves the right PTZ to the given position
-- @b "PTZL_cmd"/std_msgs::std_msgs::PTZActuatorCmd : Moves the left PTZ to the given position
+- @b "PTZR_cmd"/axis_cam::axis_cam::PTZActuatorCmd : Moves the right PTZ to the given position
+- @b "PTZL_cmd"/axis_cam::axis_cam::PTZActuatorCmd : Moves the left PTZ to the given position
@todo
@@ -95,8 +95,8 @@
//ros stuff
#include "ros/node.h"
#include "std_msgs/Image.h"
-#include "std_msgs/PTZActuatorCmd.h"
-#include "std_msgs/PTZActuatorState.h"
+#include "axis_cam/PTZActuatorCmd.h"
+#include "axis_cam/PTZActuatorState.h"
#include "rostools/Log.h"
/** Implementing launcher */
class LauncherImpl : public launcher
@@ -187,11 +187,11 @@
///Image for the right wrist
std_msgs::Image WristRImage;
///Command message shared between the PTZs
- std_msgs::PTZActuatorCmd ptz_cmd;
+ axis_cam::PTZActuatorCmd ptz_cmd;
///State of the left PTZ
- std_msgs::PTZActuatorState PTZL_state;
+ axis_cam::PTZActuatorState PTZL_state;
///State of the right PTZ
- std_msgs::PTZActuatorState PTZR_state;
+ axis_cam::PTZActuatorState PTZR_state;
///3d visualization object (irrlicht window)
Vis3d *vis3d_Window;
Modified: pkg/trunk/visualization/wx_camera_panel/manifest.xml
===================================================================
--- pkg/trunk/visualization/wx_camera_panel/manifest.xml 2008-08-28 23:01:08 UTC (rev 3771)
+++ pkg/trunk/visualization/wx_camera_panel/manifest.xml 2008-08-28 23:14:57 UTC (rev 3772)
@@ -10,6 +10,7 @@
<depend package="image_utils"/>
<depend package="roscpp"/>
<depend package="std_msgs"/>
+ <depend package="axis_cam"/>
<depend package="wx_topic_display"/>
<export>
<cpp cflags="-I${prefix}/src/" lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib -lwxcamerapanels"/>
Modified: pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp
===================================================================
--- pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp 2008-08-28 23:01:08 UTC (rev 3771)
+++ pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.cpp 2008-08-28 23:14:57 UTC (rev 3772)
@@ -278,7 +278,7 @@
{
if ( IsPTZControlEnabled() )
{
- m_ROSNode->advertise<std_msgs::PTZActuatorCmd>(m_PTZControlTopic, 0);
+ m_ROSNode->advertise<axis_cam::PTZActuatorCmd>(m_PTZControlTopic, 0);
}
}
Modified: pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.h
===================================================================
--- pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.h 2008-08-28 23:01:08 UTC (rev 3771)
+++ pkg/trunk/visualization/wx_camera_panel/src/wx_camera_panel/CameraPanel.h 2008-08-28 23:14:57 UTC (rev 3772)
@@ -36,8 +36,8 @@
// ROS includes
#include <rosthread/mutex.h>
#include <std_msgs/Image.h>
-#include <std_msgs/PTZActuatorState.h>
-#include <std_msgs/PTZActuatorCmd.h>
+#include <axis_cam/PTZActuatorState.h>
+#include <axis_cam/PTZActuatorCmd.h>
#include "image_utils/image_codec.h"
@@ -203,8 +203,8 @@
wxBitmap m_Bitmap;
bool m_RecreateBitmap;
- std_msgs::PTZActuatorState m_PTZStateMessage;
- std_msgs::PTZActuatorCmd m_PTZControlMessage;
+ axis_cam::PTZActuatorState m_PTZStateMessage;
+ axis_cam::PTZActuatorCmd m_PTZControlMessage;
/// Latest pan received from ROS
float m_CurrentPan;
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <tf...@us...> - 2008-08-29 00:55:39
|
Revision: 3784
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3784&view=rev
Author: tfoote
Date: 2008-08-29 00:55:48 +0000 (Fri, 29 Aug 2008)
Log Message:
-----------
converting Point3DFloat64 to identical message Position
Modified Paths:
--------------
pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg
pkg/trunk/vision/laser_interface/nodes/follow_light_node.py
pkg/trunk/vision/laser_interface/nodes/laser_pointer_detector_node.py
pkg/trunk/vision/laser_interface/nodes/send_command.py
pkg/trunk/vision/laser_interface/src/laser_interface/cursor3d_listener.py
pkg/trunk/vision/laser_interface/src/laser_interface/dimreduce_display.py
pkg/trunk/vision/laser_interface/src/laser_interface/test.py
pkg/trunk/vision/laser_interface/src/laser_interface/test2.py
Modified: pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg
===================================================================
--- pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg 2008-08-29 00:51:23 UTC (rev 3783)
+++ pkg/trunk/robot_msgs/msg/KinematicSpaceParameters.msg 2008-08-29 00:55:48 UTC (rev 3784)
@@ -17,8 +17,8 @@
# Lower coordinate for a box defining the volume in the workspace the
# motion planner is active in. If planning in 2D, only first 2 values
# (x, y) of the points are used.
-std_msgs/Point3DFloat64 volumeMin
+std_msgs/Position volumeMin
# Higher coordinate for the box described above
-std_msgs/Point3DFloat64 volumeMax
+std_msgs/Position volumeMax
Modified: pkg/trunk/vision/laser_interface/nodes/follow_light_node.py
===================================================================
--- pkg/trunk/vision/laser_interface/nodes/follow_light_node.py 2008-08-29 00:51:23 UTC (rev 3783)
+++ pkg/trunk/vision/laser_interface/nodes/follow_light_node.py 2008-08-29 00:55:48 UTC (rev 3784)
@@ -27,7 +27,7 @@
#
## @author Hai Nguyen/ha...@ga...
from pkg import *
-from std_msgs.msg import Point3DFloat64
+from std_msgs.msg import Position
from std_msgs.msg import BaseVel
from std_msgs.msg import RobotBase2DOdom
import sys
@@ -119,7 +119,7 @@
pub = rospy.TopicPub('cmd_vel', BaseVel)
follow_behavior = FollowBehavior(pub)
rospy.TopicSub('odom', RobotBase2DOdom, follow_behavior.robot_moved)
- rospy.TopicSub(CURSOR_TOPIC, Point3DFloat64, follow_behavior.cursor_moved)
+ rospy.TopicSub(CURSOR_TOPIC, Position, follow_behavior.cursor_moved)
rospy.ready(sys.argv[0])
#follow_behavior.cursor_moved(FakePoint(0.0,0.0,1.0))
while not rospy.isShutdown():
Modified: pkg/trunk/vision/laser_interface/nodes/laser_pointer_detector_node.py
===================================================================
--- pkg/trunk/vision/laser_interface/nodes/laser_pointer_detector_node.py 2008-08-29 00:51:23 UTC (rev 3783)
+++ pkg/trunk/vision/laser_interface/nodes/laser_pointer_detector_node.py 2008-08-29 00:55:48 UTC (rev 3784)
@@ -72,7 +72,7 @@
#- @b "laser_mode"/String : sets the mode to either 'debug' 'display' 'verbose' 'rebuild' 'positive' or 'clear'.
#
#Publishes to (name / type):
-#- @b "cursor3d"/Point3DFloat64:
+#- @b "cursor3d"/Position:
#
#<hr>
#
@@ -80,7 +80,7 @@
#
from pkg import *
-from std_msgs.msg import Point3DFloat64
+from std_msgs.msg import Position
from std_msgs.msg import String
import sys, time
import opencv as cv
@@ -294,7 +294,7 @@
rospy.TopicSub(LASER_MODE_TOPIC, String, self._mode_handler)
#Publish
- self.topic = rospy.TopicPub(CURSOR_TOPIC, Point3DFloat64)
+ self.topic = rospy.TopicPub(CURSOR_TOPIC, Position)
#Ready
rospy.ready(sys.argv[0])
@@ -368,7 +368,7 @@
if result != None:
p = result['point']
- self.topic.publish(Point3DFloat64(p[0,0], p[1,0], p[2,0]))
+ self.topic.publish(Position(p[0,0], p[1,0], p[2,0]))
if self.debug:
print '>> undistort %.2f' % (undistort_time - start_time)
@@ -576,7 +576,7 @@
# rospy.TopicSub(LASER_MODE_TOPIC, String, self._mode_handler)
#
# #Publish
-# self.topic = rospy.TopicPub(CURSOR_TOPIC, Point3DFloat64)
+# self.topic = rospy.TopicPub(CURSOR_TOPIC, Position)
#
# #Ready
# rospy.ready(sys.argv[0])
@@ -613,7 +613,7 @@
# self.video_lock.release()
# if result != None:
# p = result['point']
-# self.topic.publish(Point3DFloat64(p[0,0], p[1,0], p[2,0]))
+# self.topic.publish(Position(p[0,0], p[1,0], p[2,0]))
#
# if self.debug:
# print '>> undistort %.2f' % (undistort_time - start_time)
Modified: pkg/trunk/vision/laser_interface/nodes/send_command.py
===================================================================
--- pkg/trunk/vision/laser_interface/nodes/send_command.py 2008-08-29 00:51:23 UTC (rev 3783)
+++ pkg/trunk/vision/laser_interface/nodes/send_command.py 2008-08-29 00:55:48 UTC (rev 3784)
@@ -27,7 +27,7 @@
#
## @author Hai Nguyen/ha...@ga...
from pkg import *
-from std_msgs.msg import Point3DFloat64
+from std_msgs.msg import Position
import sys, time
import opencv as cv
import opencv.highgui as hg
@@ -38,11 +38,11 @@
from laser_detector import *
from threading import RLock
-topic = rospy.TopicPub(CURSOR_TOPIC, Point3DFloat64)
+topic = rospy.TopicPub(CURSOR_TOPIC, Position)
rospy.ready(sys.argv[0])
if not rospy.isShutdown():
time.sleep(1)
- topic.publish(Point3DFloat64(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3])))
+ topic.publish(Position(float(sys.argv[1]), float(sys.argv[2]), float(sys.argv[3])))
print 'sent message'
Modified: pkg/trunk/vision/laser_interface/src/laser_interface/cursor3d_listener.py
===================================================================
--- pkg/trunk/vision/laser_interface/src/laser_interface/cursor3d_listener.py 2008-08-29 00:51:23 UTC (rev 3783)
+++ pkg/trunk/vision/laser_interface/src/laser_interface/cursor3d_listener.py 2008-08-29 00:55:48 UTC (rev 3784)
@@ -1,12 +1,12 @@
from pkg import *
-from std_msgs.msg import Point3DFloat64
+from std_msgs.msg import Position
import sys
class Echo:
def echo(self, point):
print 'x y z', point.x, point.y, point.z
-rospy.TopicSub(CURSOR_TOPIC, Point3DFloat64, Echo().echo)
+rospy.TopicSub(CURSOR_TOPIC, Position, Echo().echo)
rospy.ready(sys.argv[0])
rospy.spin()
Modified: pkg/trunk/vision/laser_interface/src/laser_interface/dimreduce_display.py
===================================================================
--- pkg/trunk/vision/laser_interface/src/laser_interface/dimreduce_display.py 2008-08-29 00:51:23 UTC (rev 3783)
+++ pkg/trunk/vision/laser_interface/src/laser_interface/dimreduce_display.py 2008-08-29 00:55:48 UTC (rev 3784)
@@ -281,7 +281,7 @@
#
##from pkg import *
#import rospy
-#from std_msgs.msg import Point3DFloat64
+#from std_msgs.msg import Position
#from std_msgs.msg import RobotBase2DOdom
#import sys
#
Modified: pkg/trunk/vision/laser_interface/src/laser_interface/test.py
===================================================================
--- pkg/trunk/vision/laser_interface/src/laser_interface/test.py 2008-08-29 00:51:23 UTC (rev 3783)
+++ pkg/trunk/vision/laser_interface/src/laser_interface/test.py 2008-08-29 00:55:48 UTC (rev 3784)
@@ -102,7 +102,7 @@
#
##from pkg import *
#import rospy
-#from std_msgs.msg import Point3DFloat64
+#from std_msgs.msg import Position
#from std_msgs.msg import RobotBase2DOdom
#import sys
#
Modified: pkg/trunk/vision/laser_interface/src/laser_interface/test2.py
===================================================================
--- pkg/trunk/vision/laser_interface/src/laser_interface/test2.py 2008-08-29 00:51:23 UTC (rev 3783)
+++ pkg/trunk/vision/laser_interface/src/laser_interface/test2.py 2008-08-29 00:55:48 UTC (rev 3784)
@@ -39,7 +39,7 @@
#from pkg import *
import rospy
-from std_msgs.msg import Point3DFloat64
+from std_msgs.msg import Position
from std_msgs.msg import RobotBase2DOdom
from std_msgs.msg import Pose2DFloat32
import sys
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <rob...@us...> - 2008-09-01 23:02:35
|
Revision: 3857
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3857&view=rev
Author: rob_wheeler
Date: 2008-09-01 23:02:44 +0000 (Mon, 01 Sep 2008)
Log Message:
-----------
Wrap misc_utils in a namespace
Modified Paths:
--------------
pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h
pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
pkg/trunk/util/misc_utils/include/misc_utils/factory.h
pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h 2008-08-31 07:10:55 UTC (rev 3856)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/controller.h 2008-09-01 23:02:44 UTC (rev 3857)
@@ -72,7 +72,7 @@
class Controller;
-typedef Factory<Controller> ControllerFactory;
+typedef misc_utils::Factory<Controller> ControllerFactory;
#define ROS_REGISTER_CONTROLLER(c) \
controller::Controller *ROS_New_##c() { return new c(); } \
Modified: pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h
===================================================================
--- pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-08-31 07:10:55 UTC (rev 3856)
+++ pkg/trunk/mechanism/mechanism_control/include/mechanism_control/mechanism_control.h 2008-09-01 23:02:44 UTC (rev 3857)
@@ -129,7 +129,7 @@
mechanism_control::MechanismState mechanism_state_;
static const double STATE_PUBLISHING_PERIOD = 0.1; // in seconds, higher rates are useless with the current speed of the simulator
const char* const mechanism_state_topic_;
- RealtimePublisher<mechanism_control::MechanismState> publisher_;
+ misc_utils::RealtimePublisher<mechanism_control::MechanismState> publisher_;
};
#endif /* MECHANISM_CONTROL_H */
Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h 2008-08-31 07:10:55 UTC (rev 3856)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/transmission.h 2008-09-01 23:02:44 UTC (rev 3857)
@@ -46,7 +46,7 @@
class Robot;
class Transmission;
-typedef Factory<Transmission> TransmissionFactory;
+typedef misc_utils::Factory<Transmission> TransmissionFactory;
#define ROS_REGISTER_TRANSMISSION(c) \
mechanism::Transmission *ROS_New_##c() { return new c(); } \
Modified: pkg/trunk/util/misc_utils/include/misc_utils/factory.h
===================================================================
--- pkg/trunk/util/misc_utils/include/misc_utils/factory.h 2008-08-31 07:10:55 UTC (rev 3856)
+++ pkg/trunk/util/misc_utils/include/misc_utils/factory.h 2008-09-01 23:02:44 UTC (rev 3857)
@@ -33,6 +33,8 @@
#include <map>
#include <vector>
+namespace misc_utils {
+
template <class BaseResult,
class Constructor = BaseResult* (*)()>
class Factory
@@ -78,7 +80,7 @@
ConstructorMap types_;
};
+}
-
#endif
Modified: pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h
===================================================================
--- pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h 2008-08-31 07:10:55 UTC (rev 3856)
+++ pkg/trunk/util/misc_utils/include/misc_utils/realtime_publisher.h 2008-09-01 23:02:44 UTC (rev 3857)
@@ -43,6 +43,8 @@
#include <rosthread/mutex.h>
#include <rosthread/member_thread.h>
+namespace misc_utils {
+
template <class Msg>
class RealtimePublisher
{
@@ -121,4 +123,6 @@
pthread_cond_t updated_cond_;
};
+}
+
#endif
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
|
From: <stu...@us...> - 2008-09-02 17:20:34
|
Revision: 3865
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=3865&view=rev
Author: stuglaser
Date: 2008-09-02 17:20:31 +0000 (Tue, 02 Sep 2008)
Log Message:
-----------
Moved the Pid class into mechanism_model.
Modified Paths:
--------------
pkg/trunk/controllers/generic_controllers/CMakeLists.txt
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_pd_controller.h
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h
pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
Added Paths:
-----------
pkg/trunk/mechanism/mechanism_model/include/mechanism_model/pid.h
pkg/trunk/mechanism/mechanism_model/src/pid.cpp
Removed Paths:
-------------
pkg/trunk/controllers/generic_controllers/include/generic_controllers/pid.h
pkg/trunk/controllers/generic_controllers/src/pid.cpp
Modified: pkg/trunk/controllers/generic_controllers/CMakeLists.txt
===================================================================
--- pkg/trunk/controllers/generic_controllers/CMakeLists.txt 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/controllers/generic_controllers/CMakeLists.txt 2008-09-02 17:20:31 UTC (rev 3865)
@@ -3,4 +3,4 @@
rospack(generic_controllers)
genmsg()
gensrv()
-rospack_add_library(generic_controllers src/pid.cpp src/joint_effort_controller.cpp src/joint_position_controller.cpp src/joint_velocity_controller.cpp src/ramp_effort_controller.cpp src/sine_sweep_controller.cpp src/joint_pd_controller.cpp)
+rospack_add_library(generic_controllers src/joint_effort_controller.cpp src/joint_position_controller.cpp src/joint_velocity_controller.cpp src/ramp_effort_controller.cpp src/sine_sweep_controller.cpp src/joint_pd_controller.cpp)
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_pd_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_pd_controller.h 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_pd_controller.h 2008-09-02 17:20:31 UTC (rev 3865)
@@ -54,7 +54,7 @@
#include <ros/node.h>
#include <generic_controllers/controller.h>
-#include <generic_controllers/pid.h>
+#include <mechanism_model/pid.h>
// Services
#include <generic_controllers/SetPDCommand.h>
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_position_controller.h 2008-09-02 17:20:31 UTC (rev 3865)
@@ -55,7 +55,7 @@
#include <ros/node.h>
#include <generic_controllers/controller.h>
-#include <generic_controllers/pid.h>
+#include <mechanism_model/pid.h>
// Services
#include <generic_controllers/SetCommand.h>
Modified: pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/joint_velocity_controller.h 2008-09-02 17:20:31 UTC (rev 3865)
@@ -54,7 +54,7 @@
#include <ros/node.h>
#include <generic_controllers/controller.h>
-#include <generic_controllers/pid.h>
+#include <mechanism_model/pid.h>
// Services
#include <generic_controllers/SetCommand.h>
Deleted: pkg/trunk/controllers/generic_controllers/include/generic_controllers/pid.h
===================================================================
--- pkg/trunk/controllers/generic_controllers/include/generic_controllers/pid.h 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/controllers/generic_controllers/include/generic_controllers/pid.h 2008-09-02 17:20:31 UTC (rev 3865)
@@ -1,188 +0,0 @@
-/*********************************************************************
- * 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.
- *********************************************************************/
-#pragma once
-
-/***************************************************/
-/*! \class controller::Pid
- \brief A basic pid class.
-
- This class implements a generic structure that
- can be used to create a wide range of pid
- controllers. It can function independently or
- be subclassed to provide more specific controls
- based on a particular control loop.
-
- In particular, this class implements the standard
- pid equation:
-
- command = -p_term_ - i_term_ - d_term_
-
- where: <br>
- <UL TYPE="none">
- <LI> p_term_ = p_gain_ * p_error_
- <LI> i_term_ = i_gain_ * i_error_
- <LI> d_term_ = d_gain_ * d_error_
- <LI> i_error_ = i_error_ + p_error_ * dt
- <LI> d_error_ = d_error_ + (p_error_ - p_error_last_) / dt
- </UL>
-
- given:<br>
- <UL TYPE="none">
- <LI> p_error_ = p_state-p_target.
- </UL>
-
-*/
-/***************************************************/
-class TiXmlElement;
-namespace controller
-{
-
-class Pid
-{
-public:
-
- /*!
- * \brief Constructor, zeros out Pid values when created and
- * initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
- *
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param I1 The integral upper limit.
- * \param I2 The integral lower limit.
- */
- Pid(double P = 0.0, double I = 0.0, double D = 0.0, double I1 = 0.0, double I2 = -0.0);
-
- /*!
- * \brief Destructor of Pid class.
- */
- ~Pid();
-
- /*!
- * \brief Update the Pid loop with nonuniform time step size.
- *
- * \param p_error Error since last call (p_state-p_target)
- * \param dt Change in time since last call
- */
- double updatePid(double p_error, double dt);
-
- /*!
- * \brief Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2]
- *
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param I1 The integral upper limit.
- * \param I2 The integral lower limit.
- */
- void initPid(double P, double I, double D, double I1, double I2);
-
- void initXml(TiXmlElement *config);
-
- /*!
- * \brief Set current command for this PID controller
- */
- void setCurrentCmd(double cmd);
-
- /*!
- * \brief Return current command for this PID controller
- */
- double getCurrentCmd();
-
- /*!
- * \brief Return PID error terms for the controller.
- * \param pe The proportional error.
- * \param ie The integral error.
- * \param de The derivative error.
- */
- void getCurrentPIDErrors(double *pe, double *ie, double *de);
-
- /*!
- * \brief Set PID gains for the controller.
- * \param P The proportional gain.
- * \param I The integral gain.
- * \param D The derivative gain.
- * \param i_max
- * \param i_min
- */
- void setGains(double P, double I, double D, double i_max, double i_min);
-
- /*!
- * \brief Get PID gains for the controller.
- * \param p The proportional gain.
- * \param i The integral gain.
- * \param d The derivative gain.
- * \param i_max
- * \param i_mim
- */
- void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
-
- /*!
- * \brief Update the Pid loop with nonuniform time step size.
- *
- * \param error Error since last call (p_state-p_target)
- * \param error_dot d(Error)/dt since last call
- * \param dt Change in time since last call
- */
- double updatePid(double error, double error_dot, double dt);
-
- Pid &operator =(const Pid& p)
- {
- if (this == &p)
- return *this;
-
- p_gain_ = p.p_gain_;
- i_gain_ = p.i_gain_;
- d_gain_ = p.d_gain_;
- i_max_ = p.i_max_;
- i_min_ = p.i_min_;
-
- p_error_last_ = p_error_ = i_error_ = d_error = cmd_ = 0.0;
- return *this;
- }
-
-private:
- double p_error_last_; /**< _Save position state for derivative state calculation. */
- double p_error_; /**< Position error. */
- double d_error_; /**< Derivative error. */
- double i_error_; /**< Integator error. */
- double p_gain_; /**< Proportional gain. */
- double i_gain_; /**< Integral gain. */
- double d_gain_; /**< Derivative gain. */
- double i_max_; /**< Maximum allowable integral term. */
- double i_min_; /**< Minimum allowable integral term. */
- double cmd_; /**< Command to send. */
-};
-
-}
Deleted: pkg/trunk/controllers/generic_controllers/src/pid.cpp
===================================================================
--- pkg/trunk/controllers/generic_controllers/src/pid.cpp 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/controllers/generic_controllers/src/pid.cpp 2008-09-02 17:20:31 UTC (rev 3865)
@@ -1,193 +0,0 @@
-/*********************************************************************
- * 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.
- *********************************************************************/
-#include <generic_controllers/pid.h>
-#include "tinyxml/tinyxml.h"
-
-using namespace controller;
-
-Pid::Pid(double P, double I, double D, double I1, double I2) :
- p_gain_(P), i_gain_(I), d_gain_(D), i_max_(I1), i_min_(I2)
-{
- p_error_last_ = 0.0;
- p_error_ = 0.0;
- d_error_ = 0.0;
- i_error_ = 0.0;
- cmd_ = 0.0;
-}
-
-Pid::~Pid()
-{
-}
-
-void Pid::initPid(double P, double I, double D, double I1, double I2)
-{
- p_gain_ = P;
- i_gain_ = I;
- d_gain_ = D;
- i_max_ = I1;
- i_min_ = I2;
- p_error_last_ = 0.0;
- p_error_ = 0.0;
- d_error_ = 0.0;
- i_error_ = 0.0;
- cmd_ = 0.0;
-}
-
-void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
-{
- p = p_gain_;
- i = i_gain_;
- d = d_gain_;
- i_max = i_max_;
- i_min = i_min_;
-}
-
-void Pid::setGains(double P, double I, double D, double I1, double I2)
-{
- p_gain_ = P;
- i_gain_ = I;
- d_gain_ = D;
- i_max_ = I1;
- i_min_ = I2;
-}
-
-void Pid::initXml(TiXmlElement *config)
-{
- p_gain_ = config->Attribute("p") ? atof(config->Attribute("p")) : 0.0;
- i_gain_ = config->Attribute("i") ? atof(config->Attribute("i")) : 0.0;
- d_gain_ = config->Attribute("d") ? atof(config->Attribute("d")) : 0.0;
- i_max_ = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
- i_min_ = -i_max_;
-}
-
-double Pid::updatePid(double error, double dt)
-{
- double p_term, d_term, i_term;
- p_error_ = error; //this is pError = pState-pTarget
-
- if (dt == 0)
- {
- throw "dividebyzero"; //TODO: not sure how to deal with this
- }
- else
- {
- // Calculate proportional contribution to command
- p_term = p_gain_ * p_error_;
-
- // Calculate the integral error
- i_error_ = i_error_ + dt * p_error_;
-
- //Calculate integral contribution to command
- i_term = i_gain_ * i_error_;
-
- // Limit i_term so that the limit is meaningful in the output
- if (i_term > i_max_)
- {
- i_term = i_max_;
- }
- else if (i_term < i_min_)
- {
- i_term = i_min_;
- }
-
- // Calculate the derivative error
- if (dt != 0)
- {
- d_error_ = (p_error_ - p_error_last_) / dt;
- p_error_last_ = p_error_;
- }
- // Calculate derivative contribution to command
- d_term = d_gain_ * d_error_;
- cmd_ = -p_term - i_term - d_term;
- }
- return cmd_;
-}
-
-
-double Pid::updatePid(double error, double error_dot, double dt)
-{
- double p_term, d_term, i_term;
- p_error_ = error; //this is pError = pState-pTarget
- d_error_ = error_dot;
- if (dt == 0)
- {
- throw "dividebyzero"; //TODO: not sure how to deal with this
- }
- else
- {
- // Calculate proportional contribution to command
- p_term = p_gain_ * p_error_;
-
- // Calculate the integral error
- i_error_ = i_error_ + dt * p_error_;
-
- //Calculate integral contribution to command
- i_term = i_gain_ * i_error_;
-
- // Limit i_term so that the limit is meaningful in the output
- if (i_term > i_max_)
- {
- i_term = i_max_;
- }
- else if (i_term < i_min_)
- {
- i_term = i_min_;
- }
-
- // Calculate derivative contribution to command
- d_term = d_gain_ * d_error_;
- cmd_ = -p_term - i_term - d_term;
- }
- return cmd_;
-}
-
-
-
-void Pid::setCurrentCmd(double cmd)
-{
- cmd_ = cmd;
-}
-
-double Pid::getCurrentCmd()
-{
- return cmd_;
-}
-
-void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
-{
- *pe = p_error_;
- *ie = i_error_;
- *de = d_error_;
-}
-
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/include/gazebo_plugin/Pr2_Actarray.hh 2008-09-02 17:20:31 UTC (rev 3865)
@@ -32,7 +32,7 @@
#include <pr2Core/pr2Core.h>
#include <vector>
-#include <generic_controllers/pid.h>
+#include <mechanism_model/pid.h>
namespace gazebo
{
Modified: pkg/trunk/mechanism/mechanism_model/CMakeLists.txt
===================================================================
--- pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-09-02 17:17:40 UTC (rev 3864)
+++ pkg/trunk/mechanism/mechanism_model/CMakeLists.txt 2008-09-02 17:20:31 UTC (rev 3865)
@@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 2.6)
include(rosbuild)
rospack(mechanism_model)
-rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp src/robot.cpp src/link.cpp src/chain.cpp)
+rospack_add_library(mechanism_model src/simple_transmission.cpp src/joint.cpp src/robot.cpp src/link.cpp src/chain.cpp src/pid.cpp)
Copied: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/pid.h (from rev 3864, pkg/trunk/controllers/generic_controllers/include/generic_controllers/pid.h)
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/pid.h (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/pid.h 2008-09-02 17:20:31 UTC (rev 3865)
@@ -0,0 +1,188 @@
+/*********************************************************************
+ * 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.
+ *********************************************************************/
+#pragma once
+
+/***************************************************/
+/*! \class controller::Pid
+ \brief A basic pid class.
+
+ This class implements a generic structure that
+ can be used to create a wide range of pid
+ controllers. It can function independently or
+ be subclassed to provide more specific controls
+ based on a particular control loop.
+
+ In particular, this class implements the standard
+ pid equation:
+
+ command = -p_term_ - i_term_ - d_term_
+
+ where: <br>
+ <UL TYPE="none">
+ <LI> p_term_ = p_gain_ * p_error_
+ <LI> i_term_ = i_gain_ * i_error_
+ <LI> d_term_ = d_gain_ * d_error_
+ <LI> i_error_ = i_error_ + p_error_ * dt
+ <LI> d_error_ = d_error_ + (p_error_ - p_error_last_) / dt
+ </UL>
+
+ given:<br>
+ <UL TYPE="none">
+ <LI> p_error_ = p_state-p_target.
+ </UL>
+
+*/
+/***************************************************/
+class TiXmlElement;
+namespace controller
+{
+
+class Pid
+{
+public:
+
+ /*!
+ * \brief Constructor, zeros out Pid values when created and
+ * initialize Pid-gains and integral term limits:[iMax:iMin]-[I1:I2].
+ *
+ * \param P The proportional gain.
+ * \param I The integral gain.
+ * \param D The derivative gain.
+ * \param I1 The integral upper limit.
+ * \param I2 The integral lower limit.
+ */
+ Pid(double P = 0.0, double I = 0.0, double D = 0.0, double I1 = 0.0, double I2 = -0.0);
+
+ /*!
+ * \brief Destructor of Pid class.
+ */
+ ~Pid();
+
+ /*!
+ * \brief Update the Pid loop with nonuniform time step size.
+ *
+ * \param p_error Error since last call (p_state-p_target)
+ * \param dt Change in time since last call
+ */
+ double updatePid(double p_error, double dt);
+
+ /*!
+ * \brief Initialize PID-gains and integral term limits:[iMax:iMin]-[I1:I2]
+ *
+ * \param P The proportional gain.
+ * \param I The integral gain.
+ * \param D The derivative gain.
+ * \param I1 The integral upper limit.
+ * \param I2 The integral lower limit.
+ */
+ void initPid(double P, double I, double D, double I1, double I2);
+
+ void initXml(TiXmlElement *config);
+
+ /*!
+ * \brief Set current command for this PID controller
+ */
+ void setCurrentCmd(double cmd);
+
+ /*!
+ * \brief Return current command for this PID controller
+ */
+ double getCurrentCmd();
+
+ /*!
+ * \brief Return PID error terms for the controller.
+ * \param pe The proportional error.
+ * \param ie The integral error.
+ * \param de The derivative error.
+ */
+ void getCurrentPIDErrors(double *pe, double *ie, double *de);
+
+ /*!
+ * \brief Set PID gains for the controller.
+ * \param P The proportional gain.
+ * \param I The integral gain.
+ * \param D The derivative gain.
+ * \param i_max
+ * \param i_min
+ */
+ void setGains(double P, double I, double D, double i_max, double i_min);
+
+ /*!
+ * \brief Get PID gains for the controller.
+ * \param p The proportional gain.
+ * \param i The integral gain.
+ * \param d The derivative gain.
+ * \param i_max
+ * \param i_mim
+ */
+ void getGains(double &p, double &i, double &d, double &i_max, double &i_min);
+
+ /*!
+ * \brief Update the Pid loop with nonuniform time step size.
+ *
+ * \param error Error since last call (p_state-p_target)
+ * \param error_dot d(Error)/dt since last call
+ * \param dt Change in time since last call
+ */
+ double updatePid(double error, double error_dot, double dt);
+
+ Pid &operator =(const Pid& p)
+ {
+ if (this == &p)
+ return *this;
+
+ p_gain_ = p.p_gain_;
+ i_gain_ = p.i_gain_;
+ d_gain_ = p.d_gain_;
+ i_max_ = p.i_max_;
+ i_min_ = p.i_min_;
+
+ p_error_last_ = p_error_ = i_error_ = d_error_ = cmd_ = 0.0;
+ return *this;
+ }
+
+private:
+ double p_error_last_; /**< _Save position state for derivative state calculation. */
+ double p_error_; /**< Position error. */
+ double d_error_; /**< Derivative error. */
+ double i_error_; /**< Integator error. */
+ double p_gain_; /**< Proportional gain. */
+ double i_gain_; /**< Integral gain. */
+ double d_gain_; /**< Derivative gain. */
+ double i_max_; /**< Maximum allowable integral term. */
+ double i_min_; /**< Minimum allowable integral term. */
+ double cmd_; /**< Command to send. */
+};
+
+}
Copied: pkg/trunk/mechanism/mechanism_model/src/pid.cpp (from rev 3864, pkg/trunk/controllers/generic_controllers/src/pid.cpp)
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/pid.cpp (rev 0)
+++ pkg/trunk/mechanism/mechanism_model/src/pid.cpp 2008-09-02 17:20:31 UTC (rev 3865)
@@ -0,0 +1,193 @@
+/*********************************************************************
+ * 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.
+ *********************************************************************/
+#include "mechanism_model/pid.h"
+#include "tinyxml/tinyxml.h"
+
+using namespace controller;
+
+Pid::Pid(double P, double I, double D, double I1, double I2) :
+ p_gain_(P), i_gain_(I), d_gain_(D), i_max_(I1), i_min_(I2)
+{
+ p_error_last_ = 0.0;
+ p_error_ = 0.0;
+ d_error_ = 0.0;
+ i_error_ = 0.0;
+ cmd_ = 0.0;
+}
+
+Pid::~Pid()
+{
+}
+
+void Pid::initPid(double P, double I, double D, double I1, double I2)
+{
+ p_gain_ = P;
+ i_gain_ = I;
+ d_gain_ = D;
+ i_max_ = I1;
+ i_min_ = I2;
+ p_error_last_ = 0.0;
+ p_error_ = 0.0;
+ d_error_ = 0.0;
+ i_error_ = 0.0;
+ cmd_ = 0.0;
+}
+
+void Pid::getGains(double &p, double &i, double &d, double &i_max, double &i_min)
+{
+ p = p_gain_;
+ i = i_gain_;
+ d = d_gain_;
+ i_max = i_max_;
+ i_min = i_min_;
+}
+
+void Pid::setGains(double P, double I, double D, double I1, double I2)
+{
+ p_gain_ = P;
+ i_gain_ = I;
+ d_gain_ = D;
+ i_max_ = I1;
+ i_min_ = I2;
+}
+
+void Pid::initXml(TiXmlElement *config)
+{
+ p_gain_ = config->Attribute("p") ? atof(config->Attribute("p")) : 0.0;
+ i_gain_ = config->Attribute("i") ? atof(config->Attribute("i")) : 0.0;
+ d_gain_ = config->Attribute("d") ? atof(config->Attribute("d")) : 0.0;
+ i_max_ = config->Attribute("iClamp") ? atof(config->Attribute("iClamp")) : 0.0;
+ i_min_ = -i_max_;
+}
+
+double Pid::updatePid(double error, double dt)
+{
+ double p_term, d_term, i_term;
+ p_error_ = error; //this is pError = pState-pTarget
+
+ if (dt == 0)
+ {
+ throw "dividebyzero"; //TODO: not sure how to deal with this
+ }
+ else
+ {
+ // Calculate proportional contribution to command
+ p_term = p_gain_ * p_error_;
+
+ // Calculate the integral error
+ i_error_ = i_error_ + dt * p_error_;
+
+ //Calculate integral contribution to command
+ i_term = i_gain_ * i_error_;
+
+ // Limit i_term so that the limit is meaningful in the output
+ if (i_term > i_max_)
+ {
+ i_term = i_max_;
+ }
+ else if (i_term < i_min_)
+ {
+ i_term = i_min_;
+ }
+
+ // Calculate the derivative error
+ if (dt != 0)
+ {
+ d_error_ = (p_error_ - p_error_last_) / dt;
+ p_error_last_ = p_error_;
+ }
+ // Calculate derivative contribution to command
+ d_term = d_gain_ * d_error_;
+ cmd_ = -p_term - i_term - d_term;
+ }
+ return cmd_;
+}
+
+
+double Pid::updatePid(double error, double error_dot, double dt)
+{
+ double p_term, d_term, i_term;
+ p_error_ = error; //this is pError = pState-pTarget
+ d_error_ = error_dot;
+ if (dt == 0)
+ {
+ throw "dividebyzero"; //TODO: not sure how to deal with this
+ }
+ else
+ {
+ // Calculate proportional contribution to command
+ p_term = p_gain_ * p_error_;
+
+ // Calculate the integral error
+ i_error_ = i_error_ + dt * p_error_;
+
+ //Calculate integral contribution to command
+ i_term = i_gain_ * i_error_;
+
+ // Limit i_term so that the limit is meaningful in the output
+ if (i_term > i_max_)
+ {
+ i_term = i_max_;
+ }
+ else if (i_term < i_min_)
+ {
+ i_term = i_min_;
+ }
+
+ // Calculate derivative contribution to command
+ d_term = d_gain_ * d_error_;
+ cmd_ = -p_term - i_term - d_term;
+ }
+ return cmd_;
+}
+
+
+
+void Pid::setCurrentCmd(double cmd)
+{
+ cmd_ = cmd;
+}
+
+double Pid::getCurrentCmd()
+{
+ return cmd_;
+}
+
+void Pid::getCurrentPIDErrors(double *pe, double *ie, double *de)
+{
+ *pe = p_error_;
+ *ie = i_error_;
+ *de = d_error_;
+}
+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
Thanks for helping keep SourceForge clean.
X