[Personalrobots-commit] SF.net SVN: personalrobots:[3601] pkg/trunk

[<hsu...@us...>]

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() );
-+        renderSys->bindGpuProgramParameters(Ogre::GPT_VERTEX_PROGRAM,
-+            pass->getVertexProgramParameters());
-+      }
-+
-+      if (pass->hasFragmentProgram())
-+      {   
-+        renderSys->bindGpuProgram( pass->getFragmentProgram()->_getBindingDelegate() );
-+        renderSys->bindGpuProgramParameters(Ogre::GPT_FRAGMENT_PROGRAM, 
-+            pass->getFragmentProgramParameters());
-+      }   
-+      this->renderTargets[i]->update();
-+    }
-+
-+    sceneMgr->_suppressRenderStateChanges(false); 
-+
-+    // Render the image texture
-+    for (i=0; i<2; i++)
-     {
--      renderSys->bindGpuProgram( pass->getVertexProgram()->_getBindingDelegate() );
--      renderSys->bindGpuProgramParameters(Ogre::GPT_VERTEX_PROGRAM,
--          pass->getVertexProgramParameters());
-+      this->renderTargets[i]->update();
-     }
- 
--    if (pass->hasFragmentProgram())
--    {   
--      renderSys->bindGpuProgram( pass->getFragmentProgram()->_getBindingDelegate() );
--      renderSys->bindGpuProgramParameters(Ogre::GPT_FRAGMENT_PROGRAM, 
--          pass->getFragmentProgramParameters());
--    }   
--    this->renderTargets[i]->update();
--  }
-+    if (gridNode)
-+      gridNode->setVisible(true);
- 
--  sceneMgr->_suppressRenderStateChanges(false); 
-+    this->FillBuffers();
- 
--  // Render the image texture
--  for (i=0; i<2; i++)
--  {
--    this->renderTargets[i]->update();
-+    if (this->saveFrames)
-+      this->SaveFrame();
-   }
--
--  if (gridNode)
--    gridNode->setVisible(true);
--
--  this->FillBuffers();
--
--  if (this->saveFrames)
--    this->SaveFrame();
- }
- 
- ////////////////////////////////////////////////////////////////////////////////
 Index: server/sensors/ray/RaySensor.cc
 ===================================================================
---- server/sensors/ray/RaySensor.cc	(revision 6952)
+--- server/sensors/ray/RaySensor.cc	(revision 6981)
 +++ server/sensors/ray/RaySensor.cc	(working copy)
-@@ -237,7 +237,7 @@
+@@ -271,7 +271,7 @@
  // Update the sensor information
  void RaySensor::UpdateChild()
  {
@@ -1139,18 +843,18 @@
      Pose3d poseDelta;
 Index: server/sensors/Sensor.cc
 ===================================================================
---- server/sensors/Sensor.cc	(revision 6952)
+--- server/sensors/Sensor.cc	(revision 6981)
 +++ server/sensors/Sensor.cc	(working copy)
-@@ -32,6 +32,7 @@
- #include "World.hh"
+@@ -33,6 +33,7 @@
  #include "ControllerFactory.hh"
+ #include "Simulator.hh"
  #include "Sensor.hh"
 +#include "Simulator.hh"
  
  using namespace gazebo;
  
-@@ -58,6 +59,14 @@
-   this->SetName(node->GetString("name","",1));
+@@ -69,6 +70,14 @@
+ 
    this->LoadController( node->GetChildByNSPrefix("controller") );
    this->LoadChild(node);
 +
@@ -1164,21 +868,7 @@
  }
  
  ////////////////////////////////////////////////////////////////////////////////
-@@ -71,7 +80,12 @@
- /// Update the sensor
- void Sensor::Update()
- {
--  this->UpdateChild();
-+  if (this->lastUpdate + updatePeriod <= Simulator::Instance()->GetSimTime())
-+  {
-+    this->UpdateChild();
-+    this->lastUpdate = Simulator::Instance()->GetSimTime();
-+  }
-+  // controller has its own updatRate control
-   if (this->controller)
-     this->controller->Update();
- }
-@@ -147,4 +161,11 @@
+@@ -183,4 +192,11 @@
    this->active = value;
  }
  
@@ -1192,7 +882,7 @@
 +
 Index: server/Simulator.cc
 ===================================================================
---- server/Simulator.cc	(revision 6952)
+--- server/Simulator.cc	(revision 6981)
 +++ server/Simulator.cc	(working copy)
 @@ -72,6 +72,7 @@
    timeout(-1),
@@ -1202,7 +892,7 @@
  }
  
  ////////////////////////////////////////////////////////////////////////////////
-@@ -254,6 +255,9 @@
+@@ -291,6 +292,9 @@
    {
      currTime = this->GetRealTime();
  
@@ -1212,7 +902,7 @@
      if (physicsUpdateRate == 0 || 
          currTime - this->prevPhysicsTime >= physicsUpdatePeriod) 
      {
-@@ -278,6 +282,9 @@
+@@ -315,6 +319,9 @@
        this->prevPhysicsTime = this->GetRealTime();
      }
  
@@ -1222,219 +912,19 @@
      // Update the rendering
      if (renderUpdateRate == 0 || 
          currTime - this->prevRenderTime >= renderUpdatePeriod)
-Index: server/rendering/UserCamera.hh
-===================================================================
---- server/rendering/UserCamera.hh	(revision 6952)
-+++ server/rendering/UserCamera.hh	(working copy)
-@@ -52,7 +52,7 @@
-     public: void Load( XMLConfigNode *node );
- 
-     /// \brief Initialize
--    public: void Init();
-+    public: void Init( GLWindow *parentWindow );
- 
-     /// \brief Update
-     public: void Update();
-@@ -71,6 +71,9 @@
- 
-     /// \brief Get the average FPS
-     public: virtual float GetAvgFPS();
-+    
-+    /// \brief Save frame.
-+    private: void SaveFrame();
- 
-     /// Pointer to the viewport
-     protected: Ogre::Viewport *viewport;
-@@ -80,6 +83,9 @@
- 
-     private: std::string name;
-     private: static unsigned int cameraCount;
-+    private: unsigned int windowWidth;
-+    private: unsigned int windowHeight;
-+    private: bool loaded;
-   };
- }
- 
-Index: server/rendering/UserCamera.cc
-===================================================================
---- server/rendering/UserCamera.cc	(revision 6952)
-+++ server/rendering/UserCamera.cc	(working copy)
-@@ -25,6 +25,7 @@
-  */
- 
- #include <Ogre.h>
-+#include <OgreImageCodec.h>
- #include <sstream>
- 
- #include "Global.hh"
-@@ -33,16 +34,19 @@
- #include "OgreAdaptor.hh"
- #include "OgreCreator.hh"
- #include "UserCamera.hh"   
-+#include "OgreVisual.hh"
-+#include "OgreFrameListener.hh"
- 
- using namespace gazebo;
- 
- ////////////////////////////////////////////////////////////////////////////////
- /// Constructor
- UserCamera::UserCamera(GLWindow *parentWindow)
--  : OgreCamera("UserCamera")
-+   : OgreCamera("User"), windowWidth(0), windowHeight(0), loaded(false)
-+//  : OgreCamera("UserCamera")
- {
--  this->window = OgreCreator::CreateWindow(parentWindow, 
--                     parentWindow->w(), parentWindow->h());
-+//  this->window = OgreCreator::CreateWindow(parentWindow, 
-+//                     parentWindow->w(), parentWindow->h());
- }
- 
- ////////////////////////////////////////////////////////////////////////////////
-@@ -56,15 +60,23 @@
- void UserCamera::Load(XMLConfigNode *node)
- {
-   OgreCamera::LoadCam(node);
--
-   this->SetClipDist(0.1, 100);
-   this->SetFOV( DTOR(60) );
-+  loaded = true;
- }
- 
- ////////////////////////////////////////////////////////////////////////////////
- /// Initialize
--void UserCamera::Init()
-+void UserCamera::Init(GLWindow *parentWindow)
- {
-+  if (!loaded) {
-+    Load(NULL);
-+  }
-+
-+
-+  this->window = OgreCreator::CreateWindow(parentWindow, 
-+                     parentWindow->w(), parentWindow->h());
-+
-   this->SetCameraSceneNode( OgreAdaptor::Instance()->sceneMgr->getRootSceneNode()->createChildSceneNode( this->GetCameraName() + "_SceneNode") );
- 
-   OgreCamera::InitCam();
-@@ -75,6 +87,8 @@
-   this->SetAspectRatio( Ogre::Real(this->viewport->getActualWidth()) / Ogre::Real(this->viewport->getActualHeight()) );
- 
-   this->viewport->setVisibilityMask(this->visibilityMask);
-+  
-+  this->saveFrames = false;
- }
- 
- ////////////////////////////////////////////////////////////////////////////////
-@@ -84,6 +98,33 @@
-   OgreCamera::UpdateCam();
- 
-   OgreAdaptor::Instance()->UpdateWindow(this->window, this);
-+
-+  if (this->saveFrames && this->window)
-+  {
-+    unsigned int w = this->window->getWidth(), h = this->window->getHeight();
-+    if (w != this->windowWidth || h != this->windowHeight || !this->saveFrameBuffer) 
-+    {
-+      if (this->saveFrameBuffer)
-+	delete[] this->saveFrameBuffer;
-+      this->saveFrameBuffer = new unsigned char[w * h * 3];
-+      this->windowWidth = w;
-+      this->windowHeight = h;
-+    }
-+    unsigned char* tmpBuffer = new unsigned char[w * h * 3];
-+    glReadBuffer(GL_BACK);
-+    glPixelStorei(GL_PACK_ALIGNMENT, 1);
-+    glReadPixels(0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, tmpBuffer);
-+    unsigned int rowLength = w * 3;
-+    for (unsigned int i = 0; i < h; i++) {
-+      memcpy(&this->saveFrameBuffer[(h - i - 1) * rowLength],
-+	     &tmpBuffer[i * rowLength], rowLength);
-+    }
-+
-+
-+
-+    this->SaveFrame();
-+    delete[] tmpBuffer;
-+  }
- }
- 
- 
-@@ -119,3 +160,58 @@
- 
-   return avgFPS;
- }
-+
-+////////////////////////////////////////////////////////////////////////////////
-+/// Save a frame
-+void UserCamera::SaveFrame()
-+{
-+  if (!saveFrameBuffer || this->windowWidth == 0 || this->windowHeight == 0)
-+    return;
-+  
-+  std::ostringstream sstream;
-+  Ogre::ImageCodec::ImageData *imgData;
-+  Ogre::Codec * pCodec;
-+  size_t size, pos;
-+
-+  // Create image data structure
-+  imgData  = new Ogre::ImageCodec::ImageData();
-+
-+  imgData->width = this->windowWidth;
-+  imgData->height = this->windowHeight;
-+  imgData->depth = 1;
-+  imgData->format = Ogre::PF_B8G8R8;
-+  size = imgData->width * imgData->height * 3;
-+
-+  // Wrap buffer in a chunk
-+  Ogre::MemoryDataStreamPtr stream(new Ogre::MemoryDataStream( this->saveFrameBuffer, size, false));
-+
-+  char tmp[1024];
-+  if (!this->savePathname.empty())
-+  {
-+    sprintf(tmp, "%s/genericCam-%04d.jpg", this->savePathname.c_str(), this->saveCount);
-+  }
-+  else
-+  {
-+    sprintf(tmp, "genericCam-%04d.jpg", this->saveCount);
-+  }
-+
-+
-+  // Get codec
-+  Ogre::String filename = tmp;
-+  pos = filename.find_last_of(".");
-+  Ogre::String extension;
-+
-+  while (pos != filename.length() - 1)
-+    extension += filename[++pos];
-+
-+  // Get the codec
-+  pCodec = Ogre::Codec::getCodec(extension);
-+
-+  // Write out
-+  Ogre::Codec::CodecDataPtr codecDataPtr(imgData);
-+  pCodec->codeToFile(stream, filename, codecDataPtr);
-+
-+
-+
-+  this->saveCount++;
-+}
 Index: server/GazeboConfig.cc
 ===================================================================
---- server/GazeboConfig.cc	(revision 6952)
+--- server/GazeboConfig.cc	(revision 6981)
 +++ server/GazeboConfig.cc	(working copy)
-@@ -56,26 +56,43 @@
+@@ -67,31 +67,34 @@
+     this->gazeboPaths.push_back(gazebo_resource_path);
+   }
  
-   cfgFile.open(rcFilename.c_str(), std::ios::in);
- 
-+  char *ogre_resource_path = getenv("OGRE_RESOURCE_PATH");
-+  if(ogre_resource_path) {
-+    this->ogrePaths.push_back(ogre_resource_path);
-+  }
-+  char *gazebo_resource_path = getenv("GAZEBO_RESOURCE_PATH");
-+  if(gazebo_resource_path) {
-+    this->gazeboPaths.push_back(gazebo_resource_path);
-+  }
-+
+-  // if both paths are set, don't check the config file or use the defaults.
+-  if(ogre_resource_path && gazebo_resource_path) 
+-    return;
+-
+-
    if (cfgFile)
    {
      XMLConfig rc;
@@ -1476,7 +966,7 @@
      }
      this->RTTMode = rc.GetRootNode()->GetString("RTTMode", "PBuffer");
  
-@@ -83,9 +100,18 @@
+@@ -99,9 +102,18 @@
    else
    {
      gzmsg(0) << "Unable to find the file ~/.gazeborc. Using default paths. This may cause OGRE to fail.\n";
@@ -1498,89 +988,13 @@
      this->RTTMode="PBuffer";
    }
  }
-Index: server/gui/GLWindow.hh
-===================================================================
---- server/gui/GLWindow.hh	(revision 6952)
-+++ server/gui/GLWindow.hh	(working copy)
-@@ -63,6 +63,9 @@
-     /// \brief Initalize the gui
-     public: virtual void Init();
- 
-+    /// \brief Load the gui
-+    public: virtual void Load( XMLConfigNode *node );
-+
-     /// \brief Update the window
-     public: void Update();
- 
-Index: server/gui/GLFrame.cc
-===================================================================
---- server/gui/GLFrame.cc	(revision 6952)
-+++ server/gui/GLFrame.cc	(working copy)
-@@ -107,7 +107,7 @@
- // Load the frame
- void GLFrame::Load( XMLConfigNode *node )
- {
--
-+  this->glWindow->Load(node);
-   if (node)
-   {
-     this->startPose.pos = node->GetVector3("xyz", Vector3(0,0,0));
 Index: server/gui/GLWindow.cc
 ===================================================================
---- server/gui/GLWindow.cc	(revision 6952)
+--- server/gui/GLWindow.cc	(revision 6981)
 +++ server/gui/GLWindow.cc	(working copy)
-@@ -68,11 +68,17 @@
-   this->directionVec.x = 0;
-   this->directionVec.y = 0;
-   this->directionVec.z = 0;
-+  this->leftMousePressed = false;
-+  this->rightMousePressed = false;
-+  this->middleMousePressed = false;
- 
-+
-   this->keys.clear();
- 
-   if (activeWin == NULL)
-     activeWin = this;
-+
-+  this->userCamera = new UserCamera( this );
- }
- 
- 
-@@ -83,6 +89,14 @@
- }
- 
- ////////////////////////////////////////////////////////////////////////////////
-+// Load
-+void GLWindow::Load( XMLConfigNode *node )
-+{
-+  if (node) 
-+    this->userCamera->Load(node);
-+}
-+
-+////////////////////////////////////////////////////////////////////////////////
- // Init
- void GLWindow::Init()
- {
-@@ -90,11 +104,7 @@
-   this->mouseDrag = false;
- 
-   // Create the default camera.
--  this->userCamera = new UserCamera( this );
--  this->userCamera->Load(NULL);
--  this->userCamera->Init();
--  this->userCamera->RotatePitch( DTOR(30) );
--  this->userCamera->Translate( Vector3(-5, 0, 1) );
-+  this->userCamera->Init(this);
- 
-   this->activeCamera = this->userCamera;
- }
-@@ -219,17 +229,20 @@
-       this->activeCamera->RotateYaw(DTOR(-d.x * this->rotateAmount));
-       this->activeCamera->RotatePitch(DTOR(d.y * this->rotateAmount));
+@@ -227,21 +227,10 @@
      }
--    else if (this->rightMousePressed)
-+    if (this->rightMousePressed)
+     else if (this->rightMousePressed)
      {
 -      Model *model = dynamic_cast<Model*>(Simulator::Instance()->GetSelectedEntity());
 -      if (model)
@@ -1590,55 +1004,25 @@
 -        pose.pos.x -= d.y * 0.05;
 -        model->SetPose(pose);
 -      }
+-      else
+-      {
+-        Vector2<int> d = this->mousePos - this->prevMousePos;
+-        this->directionVec.x = 0;
+-        this->directionVec.y =  d.x * this->moveAmount;
+-        this->directionVec.z =  d.y * this->moveAmount;
+-      }
 +      Vector2<int> d = this->mousePos - this->prevMousePos;
 +      this->directionVec.x = 0;
 +      this->directionVec.y =  d.x * this->moveAmount;
 +      this->directionVec.z =  d.y * this->moveAmount;
      }
-+    if (this->middleMousePressed)
-+    {
-+      Vector2<int> d = this->mousePos - this->prevMousePos;
-+      this->directionVec.x =  d.y * this->moveAmount;
-+      this->directionVec.y =  0;
-+      this->directionVec.z =  0;
-+    }
- 
-   }
- 
-@@ -257,12 +270,28 @@
-   std::map<int,int>::iterator iter;
-   this->keys[keyNum] = 1;
- 
-+  // loop through the keys to find the modifiers -- swh
-+  float moveAmount = this->moveAmount;
-   for (iter = this->keys.begin(); iter!= this->keys.end(); iter++)
-   {
-     if (iter->second == 1)
+     else if (this->middleMousePressed)
      {
-       switch (iter->first)
-       {
-+        case FL_Control_L:
-+        case FL_Control_R:
-+          moveAmount = this->moveAmount * 10;
-+          break;
-+      }
-+    }
-+  }
-+
-+  for (iter = this->keys.begin(); iter!= this->keys.end(); iter++)
-+  {
-+    if (iter->second == 1)
-+    {
-+      switch (iter->first)
-+      {
-         case '=':
-         case '+':
-           this->moveAmount *= 2;
 Index: server/World.hh
 ===================================================================
---- server/World.hh	(revision 6952)
+--- server/World.hh	(revision 6981)
 +++ server/World.hh	(working copy)
-@@ -91,6 +91,26 @@
+@@ -92,6 +92,26 @@
    /// \return Pointer to the physics engine
    public: PhysicsEngine *GetPhysicsEngine() const;
  
@@ -1665,7 +1049,7 @@
    /// \brief Load all entities
    /// \param node XMLConfg node pointer
    /// \param parent Parent of the model to load
-@@ -185,6 +205,9 @@
+@@ -186,6 +206,9 @@
    /// Simulation interface
    private: SimulationIface *simIface;
  
@@ -1677,9 +1061,9 @@
  };
 Index: server/controllers/camera/generic/Generic_Camera.cc
 ===================================================================
---- server/controllers/camera/generic/Generic_Camera.cc	(revision 6952)
+--- server/controllers/camera/generic/Generic_Camera.cc	(revision 6981)
 +++ server/controllers/camera/generic/Generic_Camera.cc	(working copy)
-@@ -79,7 +79,24 @@
+@@ -86,7 +86,24 @@
  // Update the controller
  void Generic_Camera::UpdateChild()
  {
@@ -1707,28 +1091,32 @@
  ////////////////////////////////////////////////////////////////////////////////
 Index: server/controllers/Controller.cc
 ===================================================================
---- server/controllers/Controller.cc	(revision 6952)
+--- server/controllers/Controller.cc	(revision 6981)
 +++ server/controllers/Controller.cc	(working copy)
-@@ -67,8 +67,12 @@
+@@ -73,11 +73,15 @@
+   this->typeName = node->GetName();
  
-   this->name = node->GetString("name","",1);
- 
--  this->updatePeriod = 1.0 / (node->GetDouble("updateRate", 10) + 1e-6);
--  this->lastUpdate = -1e6;
-+  double updateRate  = node->GetDouble("updateRate", 0, 0);
+   this->nameP->Load(node);
++
+   this->updatePeriodP->Load(node);
+-  this->updatePeriod = 1.0 / (this->updatePeriodP->GetValue() + 1e-6);
++  double updateRate  = this->updatePeriodP->GetValue();
 +  if (updateRate == 0)
 +    this->updatePeriod = 0.0; // no throttling if updateRate is 0
 +  else
 +    this->updatePeriod = 1.0 / updateRate;
 +  this->lastUpdate   = Simulator::Instance()->GetSimTime();
  
+-  this->lastUpdate = -1e6;
+-
    childNode = node->GetChildByNSPrefix("interface");
  
+   // Create the interfaces
 Index: server/controllers/ptz/generic/Generic_PTZ.cc
 ===================================================================
---- server/controllers/ptz/generic/Generic_PTZ.cc	(revision 6952)
+--- server/controllers/ptz/generic/Generic_PTZ.cc	(revision 6981)
 +++ server/controllers/ptz/generic/Generic_PTZ.cc	(working copy)
-@@ -66,10 +66,10 @@
+@@ -68,10 +68,10 @@
  // Destructor
  Generic_PTZ::~Generic_PTZ()
  {
@@ -1745,7 +1133,7 @@
    this->tiltJoint = NULL;
 Index: server/World.cc
 ===================================================================
---- server/World.cc	(revision 6952)
+--- server/World.cc	(revision 6981)
 +++ server/World.cc	(working copy)
 @@ -27,6 +27,7 @@
  #include <assert.h>
@@ -1765,7 +1153,7 @@
  }
  
  ////////////////////////////////////////////////////////////////////////////////
-@@ -167,6 +171,7 @@
+@@ -172,6 +176,7 @@
  
    this->physicsEngine->Init();
  
@@ -1773,7 +1161,7 @@
    this->toAddModels.clear();
    this->toDeleteModels.clear();
  
-@@ -180,6 +185,11 @@
+@@ -185,6 +190,11 @@
    std::vector< Model* >::iterator miter;
    std::vector< Model* >::iterator miter2;
  
@@ -1785,7 +1173,7 @@
    // Update all the models
    for (miter=this->models.begin(); miter!=this->models.end(); miter++)
    {
-@@ -189,14 +199,27 @@
+@@ -194,14 +204,27 @@
      }
    }
  
@@ -1813,7 +1201,7 @@
    // Copy the newly created models into the main model vector
    std::copy(this->toAddModels.begin(), this->toAddModels.end(),
              std::back_inserter(this->models));
-@@ -214,6 +237,9 @@
+@@ -219,6 +242,9 @@
  
    this->toDeleteModels.clear();
  
@@ -1823,7 +1211,7 @@
    return 0;
  }
  
-@@ -268,6 +294,41 @@
+@@ -273,6 +299,41 @@
    return this->physicsEngine;
  }
  
@@ -1867,65 +1255,18 @@
  int World::LoadEntities(XMLConfigNode *node, Model *parent)
 Index: SConstruct
 ===================================================================
---- SConstruct	(revision 6952)
+--- SConstruct	(revision 6981)
 +++ SConstruct	(working copy)
 @@ -22,8 +22,11 @@
  # 3rd party packages
  #
  parseConfigs=['pkg-config --cflags --libs OGRE',
-+      	      'rospack export/cpp/cflags opende',
-+      	      'rospack export/cpp/lflags opende',
-+      	      'rospack export/cpp/cflags freeimage',
-+      	      'rospack export/cpp/lflags freeimage',
++              'rospack export/cpp/cflags opende',
++              'rospack export/cpp/lflags opende',
++              'rospack export/cpp/cflags freeimage',
++              'rospack export/cpp/lflags freeimage',
                'xml2-config --cflags --libs', 
 -      	      'ode-config --cflags --libs',
                'fltk-config --cflags --libs --ldflags --use-gl --use-images',
                'pkg-config --cflags --libs xft'
                ]
-@@ -80,7 +83,8 @@
- rcconfig = env.RCConfig(target='gazeborc', source=Value(install_prefix))
- 
- # DEFAULT list of subdirectories to build
--subdirs = ['libgazebo','server', 'player']
-+#subdirs = ['libgazebo','server', 'player']
-+subdirs = ['libgazebo','server']
- 
- # Set the compile mode
- if env['mode'] == 'debug':
-@@ -129,6 +133,21 @@
-         print "http://www.ogre3d.org/phpBB2addons/viewtopic.php?t=3293"
-         Exit(1)
-  
-+
-+  #FIXME: if this check fails, it makes it fail the check for ODE  
-+  # This test should be done outside of the configure context below, because
-+  # otherwise it tries to link against the not-yet-built libgazebo, causing
-+  # the test to always fail.
-+  simpleenv = Environment()
-+  simpleconf = Configure(env)
-+  if not simpleconf.CheckLibWithHeader('ltdl','ltdl.h','CXX'):
-+    print "  Warning: Failed to find ltdl, no plugin support will be included"
-+    env["HAVE_LTDL"]=False
-+  else:
-+    env["HAVE_LTDL"]=True
-+    env["CCFLAGS"].append("-DHAVE_LTDL")
-+  simpleconf.Finish()
-+   
-   conf = Configure(env, custom_tests = {'CheckODELib' : CheckODELib})
-    
-   #Check for the ODE library and header
-@@ -136,14 +155,6 @@
-     print "  Error: Install ODE (http://www.ode.org)"
-     Exit(1)
-   
--  #FIXME: if this check fails, it makes it fail the check for ODE  
--  if not conf.CheckLibWithHeader('ltdl','ltdl.h','CXX'):
--    print "  Warning: Failed to find ltdl, no plugin support will be included"
--    env["HAVE_LTDL"]=False
--  else:
--    env["HAVE_LTDL"]=True
--    env["CCFLAGS"].append("-DHAVE_LTDL")
--   
-   # Check for trimesh support in ODE
-   if not conf.CheckODELib():
-     print '  Error: ODE not compiled with trimesh support.'

Modified: pkg/trunk/3rdparty/gazebo/patches/camera_saveframe_tony.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/patches/camera_saveframe_tony.diff	2008-08-26 02:11:58 UTC (rev 3600)
+++ pkg/trunk/3rdparty/gazebo/patches/camera_saveframe_tony.diff	2008-08-26 02:26:26 UTC (rev 3601)
@@ -1,6 +1,6 @@
 Index: server/rendering/UserCamera.hh
 ===================================================================
---- server/rendering/UserCamera.hh	(revision 6897)
+--- server/rendering/UserCamera.hh	(revision 6915)
 +++ server/rendering/UserCamera.hh	(working copy)
 @@ -52,7 +52,7 @@
      public: void Load( XMLConfigNode *node );
@@ -21,19 +21,20 @@
  
      /// Pointer to the viewport
      protected: Ogre::Viewport *viewport;
-@@ -80,6 +83,9 @@
+@@ -80,6 +83,10 @@
  
      private: std::string name;
      private: static unsigned int cameraCount;
 +    private: unsigned int windowWidth;
 +    private: unsigned int windowHeight;
 +    private: bool loaded;
++    private: bool saveFramesDefault;
    };
  }
  
 Index: server/rendering/UserCamera.cc
 ===================================================================
---- server/rendering/UserCamera.cc	(revision 6897)
+--- server/rendering/UserCamera.cc	(revision 6915)
 +++ server/rendering/UserCamera.cc	(working copy)
 @@ -25,6 +25,7 @@
   */
@@ -43,7 +44,7 @@
  #include <sstream>
  
  #include "Global.hh"
-@@ -33,16 +34,16 @@
+@@ -33,16 +34,19 @@
  #include "OgreAdaptor.hh"
  #include "OgreCreator.hh"
  #include "UserCamera.hh"   
@@ -55,21 +56,25 @@
  ////////////////////////////////////////////////////////////////////////////////
  /// Constructor
  UserCamera::UserCamera(GLWindow *parentWindow)
--  : OgreCamera("User")
-+  : OgreCamera("User"), windowWidth(0), windowHeight(0), loaded(false)
+-  : OgreCamera("UserCamera")
++   : OgreCamera("User"), windowWidth(0), windowHeight(0), loaded(false)
++//  : OgreCamera("UserCamera")
  {
 -  this->window = OgreCreator::CreateWindow(parentWindow, 
 -                     parentWindow->w(), parentWindow->h());
++//  this->window = OgreCreator::CreateWindow(parentWindow, 
++//                     parentWindow->w(), parentWindow->h());
  }
  
  ////////////////////////////////////////////////////////////////////////////////
-@@ -56,15 +57,23 @@
+@@ -56,15 +60,24 @@
  void UserCamera::Load(XMLConfigNode *node)
  {
    OgreCamera::LoadCam(node);
 -
    this->SetClipDist(0.1, 100);
    this->SetFOV( DTOR(60) );
++  this->saveFramesDefault = node->GetBool("startSaveFrames",false,0);
 +  loaded = true;
  }
  
@@ -89,16 +94,16 @@
    this->SetCameraSceneNode( OgreAdaptor::Instance()->sceneMgr->getRootSceneNode()->createChildSceneNode( this->GetCameraName() + "_SceneNode") );
  
    OgreCamera::InitCam();
-@@ -75,6 +84,8 @@
+@@ -75,6 +88,8 @@
    this->SetAspectRatio( Ogre::Real(this->viewport->getActualWidth()) / Ogre::Real(this->viewport->getActualHeight()) );
  
    this->viewport->setVisibilityMask(this->visibilityMask);
 +  
-+  this->saveFrames = false;
++  this->saveFrames = this->saveFramesDefault;
  }
  
  ////////////////////////////////////////////////////////////////////////////////
-@@ -84,6 +95,33 @@
+@@ -84,6 +99,33 @@
    OgreCamera::UpdateCam();
  
    OgreAdaptor::Instance()->UpdateWindow(this->window, this);
@@ -132,7 +137,7 @@
  }
  
  
-@@ -119,3 +157,58 @@
+@@ -119,3 +161,58 @@
  
    return avgFPS;
  }
@@ -193,7 +198,7 @@
 +}
 Index: server/gui/GLWindow.hh
 ===================================================================
---- server/gui/GLWindow.hh	(revision 6897)
+--- server/gui/GLWindow.hh	(revision 6915)
 +++ server/gui/GLWindow.hh	(working copy)
 @@ -63,6 +63,9 @@
      /// \brief Initalize the gui
@@ -207,7 +212,7 @@
  
 Index: server/gui/GLFrame.cc
 ===================================================================
---- server/gui/GLFrame.cc	(revision 6897)
+--- server/gui/GLFrame.cc	(revision 6915)
 +++ server/gui/GLFrame.cc	(working copy)
 @@ -107,7 +107,7 @@
  // Load the frame
@@ -220,7 +225,7 @@
      this->startPose.pos = node->GetVector3("xyz", Vector3(0,0,0));
 Index: server/gui/GLWindow.hh
 ===================================================================
---- server/gui/GLWindow.hh	(revision 6897)
+--- server/gui/GLWindow.hh	(revision 6915)
 +++ server/gui/GLWindow.hh	(working copy)
 @@ -63,6 +63,9 @@
      /// \brief Initalize the gui
@@ -234,9 +239,9 @@
  
 Index: server/gui/GLWindow.cc
 ===================================================================
---- server/gui/GLWindow.cc	(revision 6897)
+--- server/gui/GLWindow.cc	(revision 6915)
 +++ server/gui/GLWindow.cc	(working copy)
-@@ -66,11 +66,17 @@
+@@ -68,11 +68,17 @@
    this->directionVec.x = 0;
    this->directionVec.y = 0;
    this->directionVec.z = 0;
@@ -254,11 +259,9 @@
  }
  
  
-@@ -79,8 +85,14 @@
- GLWindow::~GLWindow()
- {
+@@ -83,6 +89,14 @@
  }
--
+ 
  ////////////////////////////////////////////////////////////////////////////////
 +// Load
 +void GLWindow::Load( XMLConfigNode *node )
@@ -266,11 +269,12 @@
 +  if (node) 
 +    this->userCamera->Load(node);
 +}
++
 +////////////////////////////////////////////////////////////////////////////////
  // Init
  void GLWindow::Init()
  {
-@@ -88,11 +100,7 @@
+@@ -90,11 +104,7 @@
    this->mouseDrag = false;
  
    // Create the default camera.
@@ -283,17 +287,26 @@
  
    this->activeCamera = this->userCamera;
  }
-@@ -213,6 +221,21 @@
+@@ -219,17 +229,20 @@
        this->activeCamera->RotateYaw(DTOR(-d.x * this->rotateAmount));
        this->activeCamera->RotatePitch(DTOR(d.y * this->rotateAmount));
      }
+-    else if (this->rightMousePressed)
 +    if (this->rightMousePressed)
-+    {
+     {
+-      Model *model = dynamic_cast<Model*>(Simulator::Instance()->GetSelectedEntity());
+-      if (model)
+-      {
+-        Pose3d pose = model->GetPose();
+-        pose.pos.y -= d.x * 0.05;
+-        pose.pos.x -= d.y * 0.05;
+-        model->SetPose(pose);
+-      }
 +      Vector2<int> d = this->mousePos - this->prevMousePos;
 +      this->directionVec.x = 0;
 +      this->directionVec.y =  d.x * this->moveAmount;
 +      this->directionVec.z =  d.y * this->moveAmount;
-+    }
+     }
 +    if (this->middleMousePressed)
 +    {
 +      Vector2<int> d = this->mousePos - this->prevMousePos;
@@ -301,11 +314,10 @@
 +      this->directionVec.y =  0;
 +      this->directionVec.z =  0;
 +    }
-+
+ 
    }
  
-   this->mouseDrag = true;
-@@ -225,12 +248,28 @@
+@@ -257,12 +270,28 @@
    std::map<int,int>::iterator iter;
    this->keys[keyNum] = 1;
  

Modified: pkg/trunk/3rdparty/opende/Makefile
===================================================================
--- pkg/trunk/3rdparty/opende/Makefile	2008-08-26 02:11:58 UTC (rev 3600)
+++ pkg/trunk/3rdparty/opende/Makefile	2008-08-26 02:26:26 UTC (rev 3601)
@@ -2,7 +2,7 @@
 WITH_DRAWSTUFF = yes
 
 CFG_OPTIONS = --with-arch=nocona --enable-release --disable-asserts --with-pic --enable-double-precision
-SVN_REVISION = -r 1540
+SVN_REVISION = -r 1542
 
 ifeq ($(WITH_DRAWSTUFF), yes)
 CFG_OPTIONS += --with-drawstuff=X11

Modified: pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml
===================================================================
--- pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml	2008-08-26 02:11:58 UTC (rev 3600)
+++ pkg/trunk/demos/2dnav-gazebo/2dnav-gazebo.xml	2008-08-26 02:26:26 UTC (rev 3601)
@@ -1,11 +1,11 @@
 <launch>
   <group name="wg">
     <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" />
-    <node pkg="amcl_player" type="amcl_player" args="scan:=base_scan" 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" />
+    <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="amcl_player" type="amcl_player" args="scan:=base_scan" respawn="false" output="screen" />
+    <node pkg="wavefront_player" type="wavefron...
 
[truncated message content]