Revision: 8718
          http://playerstage.svn.sourceforge.net/playerstage/?rev=8718&view=rev
Author:   natepak
Date:     2010-05-28 02:43:44 +0000 (Fri, 28 May 2010)

Log Message:
-----------
More benchmarks and visualizations

Modified Paths:
--------------
    code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc
    code/gazebo/branches/simpar/libgazebo/Graphics3dIface.cc
    code/gazebo/branches/simpar/libgazebo/SimIface.cc
    code/gazebo/branches/simpar/libgazebo/gz.h
    code/gazebo/branches/simpar/server/GraphicsIfaceHandler.cc
    code/gazebo/branches/simpar/server/World.cc
    code/gazebo/branches/simpar/server/physics/ode/ODEJoint.cc
    code/gazebo/branches/simpar/server/rendering/OgreVisual.cc
    code/gazebo/branches/simpar/server/rendering/OgreVisual.hh
    code/gazebo/branches/simpar/worlds/pioneer2dx.world

Modified: code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc
===================================================================
--- code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc  
2010-05-27 19:02:34 UTC (rev 8717)
+++ code/gazebo/branches/simpar/benchmarks/pioneer_circle_benchmark.cc  
2010-05-28 02:43:44 UTC (rev 8718)
@@ -3,10 +3,12 @@
 #include <stdlib.h>
 #include <vector>
 #include <libgazebo/gz.h>
+#include <boost/lexical_cast.hpp>
 
 gazebo::Client *client = NULL;
 gazebo::SimulationIface *simIface = NULL;
 gazebo::FactoryIface *factoryIface = NULL;
+gazebo::Graphics3dIface *graphicsIface = NULL;
 
 std::string test_name="Pioneer Circle Benchmark";
 std::string data_filename = "/tmp/pioneer_circle_benchmark.data";
@@ -16,11 +18,149 @@
   std::ostringstream model;
 
   model << "<model:physical name='pioneer'>";
-  model << "  <xyz>0 0 1.145</xyz>";
-  model << "  <rpy>0.0 0.0 90.0</rpy>";
-  model << "  <include embedded='true'>";
-  model << "    <xi:include 
href='/home/nate/local/share/gazebo/worlds/models/pioneer2dx.model'/>";
-  model << "  </include>";
+  model << "  <xyz>0 0 0.15</xyz>";
+  model << "  <rpy>0.0 0.0 0.0</rpy>";
+  model << " <canonicalBody>chassis_body</canonicalBody>";
+  model << " <body:box name='chassis_body'>";
+  model << "   <xyz>0.0 0.0 0.0</xyz>";
+  model << "   <rpy>0.0 0.0 0.0</rpy>";
+  model << "   <geom:box name='chassis_geom'>";
+  model << "     <xyz>0 0 0.0</xyz>";
+  model << "     <size>0.445 0.277 0.17</size>";
+  model << "     <mass>2.0</mass>";
+  model << "     <mu1>1</mu1>";
+  model << "     <visual>";
+  model << "       <size>0.5 0.277 0.17</size>";
+  model << "       <xyz>0 0 0.04</xyz>";
+  model << "       <rpy>0 180 0</rpy>";
+  model << "       <mesh>Pioneer2dx/chassis.mesh</mesh>";
+  model << "       <material>Gazebo/Pioneer2Body</material>";
+  model << "     </visual>";
+  model << "     <visual>";
+  model << "       <xyz>0.015 0 0.09</xyz>";
+  model << "       <rpy>0 0 0</rpy>";
+  model << "       <mesh>Pioneer2at/chassis_top.mesh</mesh>";
+  model << "       <material>Gazebo/Black</material>";
+  model << "     </visual>";
+  model << "     <visual>";
+  model << "       <xyz>0.21 0.0 0.068</xyz>";
+  model << "       <rpy>0 0 0</rpy>";
+  model << "       <size>0.12 0.29 0.034</size>";
+  model << "       <mesh>Pioneer2at/sonarbank.mesh</mesh>";
+  model << "       <material>Gazebo/Gold</material>";
+  model << "     </visual>";
+  model << "     <visual>";
+  model << "       <xyz>-0.178 0.0 0.068</xyz>";
+  model << "       <rpy>0 0 180</rpy>";
+  model << "       <size>0.12 0.29 0.034</size>";
+  model << "       <mesh>Pioneer2at/sonarbank.mesh</mesh>";
+  model << "       <material>Gazebo/Gold</material>";
+  model << "     </visual>";
+  model << "   </geom:box>";
+  model << " </body:box>";
+  model << " <body:cylinder name='left_wheel'>";
+  model << "   <xyz>0.1 -0.17 -0.0725</xyz>";
+  model << "   <rpy>0 90 90</rpy>";
+  model << "   <geom:cylinder name='left_wheel_geom'>";
+  model << "     <size>0.075 0.05</size>";
+  model << "     <mass>0.5</mass>";
+  model << "     <mu1>0.5</mu1>";
+  model << "     <visual>";
+  model << "       <rpy>-90 0 0</rpy>";
+  model << "       <size>0.15 0.05 0.15</size>";
+  model << "       <mesh>Pioneer2dx/tire.mesh</mesh>";
+  model << "       <material>Gazebo/Black</material>";
+  model << "     </visual>";
+  model << "     <visual>";
+  model << "       <rpy>-90 0 0</rpy>";
+  model << "       <size>0.088 0.05 0.088</size>";
+  model << "       <mesh>Pioneer2at/wheel.mesh</mesh>";
+  model << "       <material>Gazebo/Gold</material>";
+  model << "     </visual>";
+  model << "     <visual>";
+  model << "       <rpy>0 0 0</rpy>";
+  model << "       <xyz>0 0 0.015</xyz>";
+  model << "       <size>0.04 0.04 0.08 </size>";
+  model << "       <mesh>unit_cylinder</mesh>";
+  model << "       <material>Gazebo/Black</material>";
+  model << "     </visual>";
+  model << "   </geom:cylinder>";
+  model << " </body:cylinder>";
+  model << " <body:cylinder name='right_wheel'>";
+  model << "   <xyz>0.1 0.17 -0.0725</xyz>";
+  model << "   <rpy>0 90 90</rpy>";
+  model << "   <geom:cylinder name='right_wheel_geom'>";
+  model << "     <size>0.075 0.05</size>";
+  model << "     <mass>0.5</mass>";
+  model << "     <mu1>0.5</mu1>";
+  model << "     <visual>";
+  model << "       <rpy>-90 0 0</rpy>";
+  model << "       <size>0.15 0.05 0.15</size>";
+  model << "       <mesh>Pioneer2dx/tire.mesh</mesh>";
+  model << "       <material>Gazebo/Black</material>";
+  model << "     </visual>";
+  model << "     <visual>";
+  model << "       <rpy>-90 0 0</rpy>";
+  model << "       <size>0.088 0.05 0.088</size>";
+  model << "       <mesh>Pioneer2at/wheel.mesh</mesh>";
+  model << "       <material>Gazebo/Gold</material>";
+  model << "     </visual>";
+  model << "     <visual>";
+  model << "       <rpy>0 0 0</rpy>";
+  model << "       <xyz>0 0 -0.015</xyz>";
+  model << "       <size>0.04 0.04 0.08</size>";
+  model << "       <mesh>unit_cylinder</mesh>";
+  model << "       <material>Gazebo/Black</material>";
+  model << "     </visual>";
+  model << "   </geom:cylinder>";
+  model << " </body:cylinder>";
+  model << " <body:sphere name='castor_body'>";
+  model << "   <xyz>-0.200 0 -0.11</xyz>";
+  model << "   <rpy>0 0 0</rpy>";
+  model << "   <geom:sphere name='castor_geom'>";
+  model << "     <size>0.04</size>";
+  model << "     <mass>0.5</mass>";
+  model << "     <mu1>0.5</mu1>";
+  model << "     <visual>";
+  model << "       <scale>0.04 0.04 0.04</scale>";
+  model << "       <mesh>unit_sphere</mesh>";
+  model << "       <material>Gazebo/Black</material>";
+  model << "     </visual>";
+  model << "   </geom:sphere>";
+  model << " </body:sphere>";
+  model << " <joint:hinge name='left_wheel_hinge'>";
+  model << "   <body1>left_wheel</body1>";
+  model << "   <body2>chassis_body</body2>";
+  model << "   <anchor>left_wheel</anchor>";
+  model << "   <anchorOffset>0 0 0.04</anchorOffset>";
+  model << "   <axis>0 1 0</axis>";
+  model << "   <erp>0.8</erp>";
+  model << "   <cfm>10e-5</cfm>";
+  model << " </joint:hinge>";
+  model << " <joint:hinge name='right_wheel_hinge'>";
+  model << "   <body1>right_wheel</body1>";
+  model << "   <body2>chassis_body</body2>";
+  model << "   <anchor>right_wheel</anchor>";
+  model << "   <anchorOffset>0 0 -0.04</anchorOffset>";
+  model << "   <axis>0 1 0</axis>";
+  model << "   <erp>0.8</erp>";
+  model << "   <cfm>10e-5</cfm>";
+  model << " </joint:hinge>";
+  model << " <joint:ball name='ball_joint'>";
+  model << "   <body1>castor_body</body1>";
+  model << "   <body2>chassis_body</body2>";
+  model << "   <anchor>castor_body</anchor>";
+  model << "   <erp>0.8</erp>";
+  model << "   <cfm>10e-5</cfm>";
+  model << " </joint:ball>";
+  model << " <controller:differential_position2d name='controller1'>";
+  model << "   <leftJoint>left_wheel_hinge</leftJoint>";
+  model << "   <rightJoint>right_wheel_hinge</rightJoint>";
+  model << "   <wheelSeparation>0.39</wheelSeparation>";
+  model << "   <wheelDiameter>0.15</wheelDiameter>";
+  model << "   <torque>5</torque>";
+  model << "   <interface:position name='position_iface_0'/>";
+  model << " </controller:differential_position2d>";
   model << "</model:physical>";
 
   factoryIface->Lock(1);
@@ -28,11 +168,37 @@
   factoryIface->Unlock();
 }
 
+void RunSim(double stepTime)
+{
+  simIface->StartLogEntity("pioneer");
+  simIface->Unpause();
+
+  double left = 0.1/stepTime;
+  if (left > 5)
+    left = 5.0;
+  double right = left * 0.5;
+
+  std::cout << "Left[" << left << "] Right[" << right << "]\n";
+
+  double time = simIface->data->realTime;
+  while(simIface->data->realTime - time < 60)
+  {
+    simIface->SetAngularVel("pioneer::left_wheel", gazebo::Vec3(0,0,left));
+    simIface->SetAngularVel("pioneer::right_wheel", gazebo::Vec3(0,0, right));
+    usleep(10000);
+  }
+
+  simIface->StopLogEntity("pioneer");
+  simIface->Pause();
+  simIface->Reset();
+}
+
 int main()
 {
   client = new gazebo::Client();
   simIface = new gazebo::SimulationIface();
   factoryIface = new gazebo::FactoryIface();
+  graphicsIface = new gazebo::Graphics3dIface();
 
   try
   {
@@ -68,8 +234,71 @@
   }
 
   spawn_robot();
+  usleep(100000);
 
+  /// Open the global graphics Interface
+  try
+  {
+    graphicsIface->Open(client, "pioneer");
+  }
+  catch (std::string e)
+  {
+    std::cerr << "Gazebo error: Unable to connect to an interface\n" 
+              << e << "\n";
+    return -1;
+  }
+
+  graphicsIface->DrawRibbonTrail("trail");
+
+  std::vector<std::string > step_types;
+  std::vector<std::string >::iterator iter;
+  //step_types.push_back("robust");
+  step_types.push_back("world");
+  step_types.push_back("quick");
+
+  for (iter = step_types.begin(); iter != step_types.end(); iter++)
+  {
+    std::string path = 
std::string("/home/nate/work/simpar/data/pioneer_circle/") + *iter + "/";
+    system((std::string("mkdir -p ")+path).c_str());
+
+
+    FILE *out = fopen(std::string(path+"index.txt").c_str(), "w");
+    fprintf(out,"# index step_time iterations\n");
+
+    int i = 0;
+
+    simIface->SetStepType(*iter);
+    for (double step=0.01; step > 1e-5; step *= 0.5)
+    {
+      unsigned int iterations = 10;
+      if (*iter == "world")
+        iterations = 199;
+      for (; iterations < 200; iterations +=20, i++)
+      {
+        simIface->SetStepTime(step);
+        simIface->SetStepIterations(iterations);
+
+        fprintf(out,"%d %f %d\n",i,step, iterations);
+        std::cout << "Type[" << *iter << "] Step[" << step << "] Iterations[" 
<< iterations << "]\n";
+        RunSim(step);
+
+        std::string mv_cmd = std::string("mv /tmp/pioneer.log ") + path + 
"pioneer_circle_benchmark_" + *iter + "_" + boost::lexical_cast<std::string>(i) 
+ ".data";
+        system(mv_cmd.c_str());
+      }
+    }
+
+    fclose(out);
+  }
+
+  // This will make a spiral
+  /*for (int i=0; i < 50; i++)
+  {
+    simIface->ApplyTorque("pioneer::left_wheel",gazebo::Vec3(0,0,-50));
+    usleep(100000);
+  }*/
+
   factoryIface->Close();
   simIface->Close();
+  graphicsIface->Close();
   client->Disconnect();
 }

Modified: code/gazebo/branches/simpar/libgazebo/Graphics3dIface.cc
===================================================================
--- code/gazebo/branches/simpar/libgazebo/Graphics3dIface.cc    2010-05-27 
19:02:34 UTC (rev 8717)
+++ code/gazebo/branches/simpar/libgazebo/Graphics3dIface.cc    2010-05-28 
02:43:44 UTC (rev 8718)
@@ -190,3 +190,21 @@
 
   this->Unlock();
 }
+
+////////////////////////////////////////////////////////////////////////////////
+/// Draw a ribbon trail following an entity
+void Graphics3dIface::DrawRibbonTrail(const std::string &name)
+{
+  this->Lock(1);
+  Graphics3dDrawData *cmd = &(this->data->commands[this->data->cmdCount++]);
+
+  cmd->drawMode = Graphics3dDrawData::RIBBONTRAIL;
+
+  // Set the name of the graphics drawable
+  memset( cmd->name, 0, GAZEBO_GRAPHICS3D_MAX_NAME);
+  strcpy( cmd->name, name.c_str());
+
+  cmd->intVar = 1;
+
+  this->Unlock();
+}

Modified: code/gazebo/branches/simpar/libgazebo/SimIface.cc
===================================================================
--- code/gazebo/branches/simpar/libgazebo/SimIface.cc   2010-05-27 19:02:34 UTC 
(rev 8717)
+++ code/gazebo/branches/simpar/libgazebo/SimIface.cc   2010-05-28 02:43:44 UTC 
(rev 8718)
@@ -499,6 +499,50 @@
 }
 
 
////////////////////////////////////////////////////////////////////////////////
+/// Apply a force to an entity
+void SimulationIface::ApplyForce(const std::string &entityName, Vec3 force)
+{
+  this->Lock(1);
+  this->data->responseCount = 0;
+  SimulationRequestData *request = 
&(this->data->requests[this->data->requestCount++]);
+
+  request->type = gazebo::SimulationRequestData::APPLY_FORCE;
+  memset(request->name, 0, 512);
+  strncpy(request->name, entityName.c_str(), 512);
+  request->name[511] = '\0';
+
+  if (isnan(force.x)) force.x = 0.0;
+  if (isnan(force.y)) force.y = 0.0;
+  if (isnan(force.z)) force.z = 0.0;
+
+  request->vec3Value = force;
+
+  this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Apply a torque to an entity
+void SimulationIface::ApplyTorque(const std::string &entityName, Vec3 torque)
+{
+  this->Lock(1);
+  this->data->responseCount = 0;
+  SimulationRequestData *request = 
&(this->data->requests[this->data->requestCount++]);
+
+  request->type = gazebo::SimulationRequestData::APPLY_TORQUE;
+  memset(request->name, 0, 512);
+  strncpy(request->name, entityName.c_str(), 512);
+  request->name[511] = '\0';
+
+  if (isnan(torque.x)) torque.x = 0.0;
+  if (isnan(torque.y)) torque.y = 0.0;
+  if (isnan(torque.z)) torque.z = 0.0;
+
+  request->vec3Value = torque;
+
+  this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
 // Wait for a post on the go ack semaphore
 void SimulationIface::GoAckWait()
 {

Modified: code/gazebo/branches/simpar/libgazebo/gz.h
===================================================================
--- code/gazebo/branches/simpar/libgazebo/gz.h  2010-05-27 19:02:34 UTC (rev 
8717)
+++ code/gazebo/branches/simpar/libgazebo/gz.h  2010-05-28 02:43:44 UTC (rev 
8718)
@@ -452,7 +452,9 @@
                       SET_STEP_TIME,
                       SET_STEP_ITERS,
                       SET_STEP_TYPE,
-                      GET_STEP_TYPE
+                      GET_STEP_TYPE,
+                      APPLY_FORCE,
+                      APPLY_TORQUE
                    };
 
   public: Type type; 
@@ -607,6 +609,12 @@
   /// \brief Set the angular acceleration
   public: void SetAngularAccel(const std::string &modelName,Vec3 accel);
 
+  /// \brief Apply a force to an entity
+  public: void ApplyForce(const std::string &entityName, Vec3 force);
+
+  /// \brief Apply a torque to an entity
+  public: void ApplyTorque(const std::string &entityName, Vec3 torque);
+
   /// \brief Get the child interfaces of a model
   public: void GetChildInterfaces(const std::string &modelName);
 
@@ -952,7 +960,7 @@
   /// Type of drawing to perform
   public: enum DrawMode { POINTS, LINES, LINE_STRIP, TRIANGLES, 
TRIANGLE_STRIP, 
                           TRIANGLE_FAN, PLANE, SPHERE, CUBE, CYLINDER, CONE,
-                          BILLBOARD, TEXT, METERBAR };
+                          BILLBOARD, TEXT, METERBAR, RIBBONTRAIL };
 
   /// Drawing mode
   public: DrawMode drawMode;
@@ -983,6 +991,8 @@
   public: Vec3 size;
 
   public: float fltVar;
+
+  public: int intVar;
 };
 
 /// \brief Graphics3d interface data
@@ -1033,6 +1043,8 @@
   public: void DrawMeterBar(const char *name, Vec3 pos, Vec2 size, Color clr,
                             float percent);
 
+  /// \brief Draw a ribbon trail following an entity
+  public: void DrawRibbonTrail(const std::string &name);
 
   /// Pointer to the graphics3d data
   public: Graphics3dData *data;

Modified: code/gazebo/branches/simpar/server/GraphicsIfaceHandler.cc
===================================================================
--- code/gazebo/branches/simpar/server/GraphicsIfaceHandler.cc  2010-05-27 
19:02:34 UTC (rev 8717)
+++ code/gazebo/branches/simpar/server/GraphicsIfaceHandler.cc  2010-05-28 
02:43:44 UTC (rev 8718)
@@ -144,6 +144,10 @@
         this->DrawMeterBar(vis, &this->threeDIface->data->commands[i] );
         break;
 
+      case Graphics3dDrawData::RIBBONTRAIL:
+        vis->SetRibbonTrail(this->threeDIface->data->commands[i].intVar);
+        break;
+
       default:
         gzerr(0) << "Unknown draw mode[" 
           << this->threeDIface->data->commands[i].drawMode << "\n";

Modified: code/gazebo/branches/simpar/server/World.cc
===================================================================
--- code/gazebo/branches/simpar/server/World.cc 2010-05-27 19:02:34 UTC (rev 
8717)
+++ code/gazebo/branches/simpar/server/World.cc 2010-05-28 02:43:44 UTC (rev 
8718)
@@ -845,6 +845,47 @@
           break;
         }
 
+      case SimulationRequestData::APPLY_FORCE:
+        {
+          Entity *ent = this->GetEntityByName((char*)req->name);
+
+          if (ent && ent->GetType() == Entity::BODY)
+          {
+            Body *body = (Body*)(ent);
+
+            Vector3 force(req->vec3Value.x, req->vec3Value.y, 
req->vec3Value.z);
+
+            body->SetForce(force);
+          }
+          else
+          {
+            gzerr(0) << "Invalid body name[" << req->name 
+              << "] in simulation interface Set Force Request.\n";
+          }
+          break;
+
+        }
+
+      case SimulationRequestData::APPLY_TORQUE:
+        {
+          Entity *ent = this->GetEntityByName((char*)req->name);
+
+          if (ent && ent->GetType() == Entity::BODY)
+          {
+            Body *body = (Body*)(ent);
+
+            Vector3 torque(req->vec3Value.x,req->vec3Value.y, 
req->vec3Value.z);
+
+            body->SetTorque(torque);
+          }
+          else
+          {
+            gzerr(0) << "Invalid body name[" << req->name 
+              << "] in simulation interface Set Torque Request.\n";
+          }
+          break;
+
+        }
       case SimulationRequestData::SET_LINEAR_VEL:
         {
           Entity *ent = this->GetEntityByName((char*)req->name);
@@ -872,16 +913,21 @@
         {
           Entity *ent = this->GetEntityByName((char*)req->name);
 
-          if (ent && ent->GetType() == Entity::MODEL)
+          if (ent && (ent->GetType() == Entity::MODEL || 
+              ent->GetType() == Entity::BODY))
           {
-            Model *model = (Model*)(ent);
 
             Vector3 vel( req->modelAngularVel.x, req->modelAngularVel.y,
                          req->modelAngularVel.z);
 
-            Pose3d pose = model->GetAbsPose();
+            Pose3d pose = ent->GetAbsPose();
             vel = pose.rot.RotateVector(vel);
-            model->SetAngularVel(vel);
+
+            if (ent->GetType()==Entity::MODEL)
+              ((Model*)ent)->SetAngularVel(vel);
+            else if (ent->GetType() == Entity::BODY)
+              ((Body*)ent)->SetAngularVel(vel);
+
           }
           else
           {

Modified: code/gazebo/branches/simpar/server/physics/ode/ODEJoint.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEJoint.cc  2010-05-27 
19:02:34 UTC (rev 8717)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEJoint.cc  2010-05-28 
02:43:44 UTC (rev 8718)
@@ -200,7 +200,7 @@
 /// Set the high stop of an axis(index)
 void ODEJoint::SetHighStop(int index, Angle angle)
 {
-  switch (index)
+  /*switch (index)
   {
     case 0:
       this->SetParam(dParamHiStop, angle.GetAsRadian());
@@ -209,13 +209,14 @@
     case 2:
       this->SetParam(dParamHiStop3, angle.GetAsRadian());
   };
+*/
 }
 
 
////////////////////////////////////////////////////////////////////////////////
 /// Set the low stop of an axis(index)
 void ODEJoint::SetLowStop(int index, Angle angle)
 {
-  switch (index)
+/*  switch (index)
   {
     case 0:
       this->SetParam(dParamLoStop, angle.GetAsRadian());
@@ -224,6 +225,7 @@
     case 2:
       this->SetParam(dParamLoStop3, angle.GetAsRadian());
   };
+  */
 }
 
 
////////////////////////////////////////////////////////////////////////////////

Modified: code/gazebo/branches/simpar/server/rendering/OgreVisual.cc
===================================================================
--- code/gazebo/branches/simpar/server/rendering/OgreVisual.cc  2010-05-27 
19:02:34 UTC (rev 8717)
+++ code/gazebo/branches/simpar/server/rendering/OgreVisual.cc  2010-05-28 
02:43:44 UTC (rev 8718)
@@ -70,6 +70,15 @@
   this->visible = true;
   this->ConstructorHelper(pnode, isStatic);
 
+  this->ribbonTrail = 
(Ogre::RibbonTrail*)OgreAdaptor::Instance()->sceneMgr->createMovableObject("RibbonTrail");
+  this->ribbonTrail->setMaterialName("Gazebo/Red");
+  this->ribbonTrail->setTrailLength(200);
+  this->ribbonTrail->setMaxChainElements(1000);
+  this->ribbonTrail->setNumberOfChains(1);
+  this->ribbonTrail->setVisible(false);
+  this->ribbonTrail->setInitialWidth(0,0.05);
+  
OgreAdaptor::Instance()->sceneMgr->getRootSceneNode()->attachObject(this->ribbonTrail);
+
   RTShaderSystem::Instance()->AttachEntity(this);
 }
 
@@ -1074,3 +1083,17 @@
   this->shaderP->SetValue(shader);
   RTShaderSystem::Instance()->UpdateShaders();
 }
+
+void OgreVisual::SetRibbonTrail(bool value)
+{
+  if (value)
+  {
+    this->ribbonTrail->addNode(this->sceneNode);
+  }
+  else
+  {
+    this->ribbonTrail->removeNode(this->sceneNode);
+    this->ribbonTrail->clearChain(0);
+  }
+  this->ribbonTrail->setVisible(value);
+}

Modified: code/gazebo/branches/simpar/server/rendering/OgreVisual.hh
===================================================================
--- code/gazebo/branches/simpar/server/rendering/OgreVisual.hh  2010-05-27 
19:02:34 UTC (rev 8717)
+++ code/gazebo/branches/simpar/server/rendering/OgreVisual.hh  2010-05-28 
02:43:44 UTC (rev 8718)
@@ -184,6 +184,9 @@
     /// \brief Set the shader
     public: void SetShader(const std::string &shader);
 
+    /// \brief True on or off a ribbon trail
+    public: void SetRibbonTrail(bool value);
+
     private: Ogre::MaterialPtr origMaterial;
     private: Ogre::MaterialPtr myMaterial;
     private: std::string myMaterialName;
@@ -222,6 +225,8 @@
     private: bool visible;
 
     private: static SelectionObj *selectionObj;
+
+    private: Ogre::RibbonTrail *ribbonTrail;
   };
 }
 

Modified: code/gazebo/branches/simpar/worlds/pioneer2dx.world
===================================================================
--- code/gazebo/branches/simpar/worlds/pioneer2dx.world 2010-05-27 19:02:34 UTC 
(rev 8717)
+++ code/gazebo/branches/simpar/worlds/pioneer2dx.world 2010-05-28 02:43:44 UTC 
(rev 8718)
@@ -21,12 +21,14 @@
   <physics:ode>
     <stepTime>0.001</stepTime>
     <gravity>0 0 -9.8</gravity>
-    <cfm>10e-5</cfm>
-    <erp>0.3</erp>
-    <!-- updateRate: <0 == throttle simTime to match realTime.
-                      0 == No throttling
-                     >0 == Frequency at which to throttle the sim --> 
-    <updateRate>0</updateRate>
+    <cfm>0.0000000001</cfm>
+    <erp>0.2</erp>
+
+    <stepType>robust</stepType>
+    <stepIters>100</stepIters>
+    <stepW>1.3</stepW>
+    <contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
+    <contactSurfaceLayer>0.001</contactSurfaceLayer>
   </physics:ode>
 
   <rendering:gui>


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

------------------------------------------------------------------------------

_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit

Reply via email to