[Playerstage-commit] SF.net SVN: playerstage:[7377] code/branches/federation/gazebo

[<na...@us...>]

Revision: 7377
          http://playerstage.svn.sourceforge.net/playerstage/?rev=7377&view=rev
Author:   natepak
Date:     2009-03-08 01:56:07 +0000 (Sun, 08 Mar 2009)

Log Message:
-----------
Added the stuff from trunk

Modified Paths:
--------------
    code/branches/federation/gazebo/SConstruct
    code/branches/federation/gazebo/examples/libgazebo/simiface/SConstruct
    code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
    code/branches/federation/gazebo/libgazebo/SConscript
    code/branches/federation/gazebo/libgazebo/gazebo.h
    code/branches/federation/gazebo/server/Model.cc
    code/branches/federation/gazebo/server/Model.hh
    code/branches/federation/gazebo/server/World.cc
    code/branches/federation/gazebo/server/physics/Body.cc
    code/branches/federation/gazebo/server/physics/Body.hh
    code/branches/federation/gazebo/worlds/pioneer2dx.world

Added Paths:
-----------
    code/branches/federation/gazebo/libgazebo/SimIface.cc

Property Changed:
----------------
    code/branches/federation/gazebo/
    code/branches/federation/gazebo/player_cfgs/epuck.cfg
    code/branches/federation/gazebo/player_cfgs/epuck_single.cfg


Property changes on: code/branches/federation/gazebo
___________________________________________________________________
Added: svn:mergeinfo
   + /code/gazebo/trunk:7372-7376

Modified: code/branches/federation/gazebo/SConstruct
===================================================================
--- code/branches/federation/gazebo/SConstruct	2009-03-08 01:54:38 UTC (rev 7376)
+++ code/branches/federation/gazebo/SConstruct	2009-03-08 01:56:07 UTC (rev 7377)
@@ -224,11 +224,11 @@
   #
   env.Alias('install', install_prefix)
   env.Install(install_prefix+'/bin',gazebo)
-  env.Install(install_prefix+'/share/gazebo','Media')
   env.Install(install_prefix+'/include/gazebo',headers)
   #env.Install(install_prefix+'/lib',libgazeboServerStatic )
   env.Install(install_prefix+'/lib',libgazeboServerShared )
   env.Install(install_prefix+'/share/gazebo','worlds')
+  env.Install(install_prefix+'/share/gazebo','Media')
 else:
   print 'Configure done'
   Exit()

Modified: code/branches/federation/gazebo/examples/libgazebo/simiface/SConstruct
===================================================================
--- code/branches/federation/gazebo/examples/libgazebo/simiface/SConstruct	2009-03-08 01:54:38 UTC (rev 7376)
+++ code/branches/federation/gazebo/examples/libgazebo/simiface/SConstruct	2009-03-08 01:56:07 UTC (rev 7377)
@@ -1,10 +1,11 @@
 
 # 3rd party packages
-parseConfigs=['pkg-config --cflags --libs libgazebo',
-              'pkg-config --cflags --libs libgazeboServer',
-              'pkg-config --cflags --libs playerc++']
+parseConfigs=['pkg-config --cflags --libs libgazebo']
 
+#              'pkg-config --cflags --libs libgazeboServer',
+#              'pkg-config --cflags --libs playerc++']
 
+
 env = Environment (
   CC = 'g++',
 

Modified: code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc
===================================================================
--- code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc	2009-03-08 01:54:38 UTC (rev 7376)
+++ code/branches/federation/gazebo/examples/libgazebo/simiface/simiface.cc	2009-03-08 01:56:07 UTC (rev 7377)
@@ -1,5 +1,6 @@
+#include <string.h>
+#include <iostream>
 #include <gazebo/gazebo.h>
-#include <gazebo/GazeboError.hh>
 
 int main()
 {
@@ -13,7 +14,7 @@
   {
     client->ConnectWait(serverId, GZ_CLIENT_ID_USER_FIRST);
   }
-  catch (gazebo::GazeboError e)
+  catch (std::string e)
   {
     std::cout << "Gazebo error: Unable to connect\n" << e << "\n";
     return -1;
@@ -33,54 +34,30 @@
   //simIface->data->reset = 1;
 
   // Example of how to move a model (box1_model)
-  uint8_t name[512] = "pioneer2dx_model1";
+  char name[512] = "pioneer2dx_model1";
 
   for (int i=0; i< 10; i++)
   {
-    simIface->Lock(1);
+   
+    gazebo::Pose pose;
+    gazebo::Vec3 linearVel(0.1, 0, 0);
+    gazebo::Vec3 angularVel(0, 0, 0);
+    gazebo::Vec3 linearAccel(0, 0, 0);
+    gazebo::Vec3 angularAccel(0, 0, 0);
 
-    gazebo::SimulationRequestData *request = &(simIface->data->requests[simIface->data->requestCount++]);
-
-    request->type = gazebo::SimulationRequestData::SET_STATE;
-    memcpy(request->modelName, name, 512);
-
-    request->modelPose.pos.x = i+1;
-    request->modelPose.pos.y = 0;
-    request->modelPose.pos.z = 0;
-    request->modelPose.roll = 0;
-    request->modelPose.pitch = 0;
-    request->modelPose.yaw = 0;
-
-    request->modelLinearVel.x = 0.1;
-    request->modelLinearVel.y = 0;
-    request->modelLinearVel.z = 0;
-
-    request->modelAngularVel.x = 0.1;
-    request->modelAngularVel.y = 0;
-    request->modelAngularVel.z = 0;
-
-    /*request->type = gazebo::SimulationRequestData::SET_POSE2D;
-    memcpy(request->modelName, name, 512);
-
-    request->modelPose.pos.x = i+.1;
-    request->modelPose.pos.y = 0;
-    */
-
-    simIface->Unlock();
-
-    usleep(100000);
+    simIface->SetState(name, pose, linearVel, angularVel, 
+                       linearAccel, angularAccel );
+    usleep(9000000);
   }
 
 
   // Example of resetting the simulator
-  /*simIface->Lock(1);
-  simIface->data->reset = 1;
-  simIface->Unlock();
-  */
+  // simIface->Reset();
 
   usleep(1000000);
 
   simIface->Close();
+
   delete simIface;
   return 0;
 }

Modified: code/branches/federation/gazebo/libgazebo/SConscript
===================================================================
--- code/branches/federation/gazebo/libgazebo/SConscript	2009-03-08 01:54:38 UTC (rev 7376)
+++ code/branches/federation/gazebo/libgazebo/SConscript	2009-03-08 01:56:07 UTC (rev 7377)
@@ -22,7 +22,7 @@
 # setup a special build environment for libgazebo. Do this so we can control
 # what libgazebo depends upon
 #
-env = Environment (
+myEnv = Environment (
   CC = 'g++',
   CCFLAGS = Split ('-pthread -pipe  -W -Wall -O2'),
 
@@ -41,11 +41,11 @@
 pkgconfig = env.PkgConfig(target='libgazebo.pc', source=Value(install_prefix))
 env.Install(dir=install_prefix+'/lib/pkgconfig', source=pkgconfig)
 
-sources = Split('Server.cc Client.cc Iface.cc IfaceFactory.cc')
+sources = Split('Server.cc Client.cc Iface.cc IfaceFactory.cc SimIface.cc')
 headers = Split('gazebo.h IfaceFactory.hh')
 
-sharedLib = env.SharedLibrary('gazebo', sources)
-staticLib = env.StaticLibrary('gazebo', sources)
+sharedLib = myEnv.SharedLibrary('gazebo', sources)
+staticLib = myEnv.StaticLibrary('gazebo', sources)
 
 env.Install(install_prefix+'/lib', sharedLib)
 env.Install(install_prefix+'/lib', staticLib)

Copied: code/branches/federation/gazebo/libgazebo/SimIface.cc (from rev 7376, code/gazebo/trunk/libgazebo/SimIface.cc)
===================================================================
--- code/branches/federation/gazebo/libgazebo/SimIface.cc	                        (rev 0)
+++ code/branches/federation/gazebo/libgazebo/SimIface.cc	2009-03-08 01:56:07 UTC (rev 7377)
@@ -0,0 +1,114 @@
+#include <string.h>
+#include "gazebo.h"
+
+using namespace gazebo;
+
+////////////////////////////////////////////////////////////////////////////////
+/// Pause the simulation
+void SimulationIface::Pause()
+{
+  this->Lock(1);
+  SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]);
+  request->type = SimulationRequestData::PAUSE;
+  this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Reset the simulation
+void SimulationIface::Reset()
+{
+  this->Lock(1);
+  SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]);
+  request->type = SimulationRequestData::RESET;
+  this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Save the simulation
+void SimulationIface::Save()
+{
+  this->Lock(1);
+  SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]);
+  request->type = SimulationRequestData::SAVE;
+  this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the 3d pose of a model
+void SimulationIface::GetPose3d(const char *modelName)
+{
+  this->Lock(1);
+  SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]);
+  request->type = SimulationRequestData::GET_POSE3D;
+  memcpy(request->modelName, modelName, 512);
+
+  this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the 2d pose of a model
+void SimulationIface::GetPose2d(const char *modelName)
+{
+  this->Lock(1);
+  SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]);
+  request->type = SimulationRequestData::GET_POSE2D;
+  memcpy(request->modelName, modelName, 512);
+
+  this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the 3d pose of a model
+void SimulationIface::SetPose3d(const char *modelName, const Pose &modelPose)
+{
+  this->Lock(1);
+  SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]);
+
+  request->type = SimulationRequestData::SET_POSE3D;
+  request->modelPose = modelPose;
+
+  memcpy(request->modelName, modelName, 512);
+
+  this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the 2d pose of a model
+void SimulationIface::SetPose2d(const char *modelName, float x, float y, float yaw)
+{
+  this->Lock(1);
+  SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]);
+
+  request->type = gazebo::SimulationRequestData::SET_POSE2D;
+
+  memcpy(request->modelName, modelName, 512);
+
+  request->modelPose.pos.x = x;
+  request->modelPose.pos.y = y;
+  request->modelPose.yaw = yaw;
+
+  this->Unlock();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the complete state of a model
+void SimulationIface::SetState(const char *modelName, const Pose &modelPose, 
+    const Vec3 &linearVel, const Vec3 &angularVel, const Vec3 &linearAccel, 
+    const Vec3 &angularAccel )
+{
+  this->Lock(1);
+  SimulationRequestData *request = &(this->data->requests[this->data->requestCount++]);
+
+  request->type = gazebo::SimulationRequestData::SET_STATE;
+  memcpy(request->modelName, modelName, 512);
+
+  request->modelPose = modelPose;
+  request->modelLinearVel = linearVel;
+  request->modelAngularVel = angularVel;
+
+  request->modelLinearAccel = linearAccel;
+  request->modelAngularAccel = angularAccel;
+
+  this->Unlock();
+}
+

Modified: code/branches/federation/gazebo/libgazebo/gazebo.h
===================================================================
--- code/branches/federation/gazebo/libgazebo/gazebo.h	2009-03-08 01:54:38 UTC (rev 7376)
+++ code/branches/federation/gazebo/libgazebo/gazebo.h	2009-03-08 01:56:07 UTC (rev 7377)
@@ -57,6 +57,15 @@
 /// \brief Vector 3 class
 class Vec3
 {
+  /// \brief Default Constructor
+  public: Vec3() : x(0), y(0), z(0) {}
+          
+  /// \brief Constructor
+  public: Vec3(float _x, float _y, float _z) : x(_x), y(_y), z(_z) {}
+
+  /// \brief Copy constructor
+  public: Vec3(const Vec3 &vec) : x(vec.x), y(vec.y), z(vec.z) {}
+
   /// X value
   public: float x;
 
@@ -71,6 +80,17 @@
 /// \brief Pose class
 class Pose
 {
+  /// \brief Default constructor
+  public: Pose() : pos(0,0,0), roll(0), pitch(0), yaw(0) {}
+
+  /// \brief Constructor
+  public: Pose(const Vec3 &_pos, float _roll, float _pitch, float _yaw)
+          : pos(_pos), roll(_roll), pitch(_pitch), yaw(_yaw) {} 
+
+  /// \brief Copy constructor
+  public: Pose(const Pose &pose)
+          : pos(pose.pos), roll(pose.roll), pitch(pose.pitch), yaw(pose.yaw) {} 
+
   /// 3d position
   public: Vec3 pos;
 
@@ -381,7 +401,6 @@
   public: Type type; 
   public: char modelName[512];
   public: Pose modelPose;
-
   public: Vec3 modelLinearVel;
   public: Vec3 modelAngularVel;
   public: Vec3 modelLinearAccel;
@@ -412,6 +431,7 @@
   /// Array of request responses from the simulator
   public: SimulationRequestData responses[GAZEBO_SIMULATION_MAX_REQUESTS];
   public: unsigned int responseCount;
+
 };
 
 /// \brief Common simulation interface
@@ -441,6 +461,32 @@
             this->data = (SimulationData*)((char*)this->mMap+sizeof(SimulationIface)); 
           }
 
+  /// \brief Pause the simulation
+  public: void Pause();
+
+  /// \brief Reset the simulation
+  public: void Reset();
+
+  /// \brief Save the simulation
+  public: void Save();
+
+  /// \brief Get the 3d pose of a model
+  public: void GetPose3d(const char *modelName);
+
+  /// \brief Get the 2d pose of a model
+  public: void GetPose2d(const char *modelName);
+
+  /// \brief Set the 3d pose of a model
+  public: void SetPose3d(const char *modelName, const Pose &modelPose);
+
+  /// \brief Set the 2d pose of a model
+  public: void SetPose2d(const char *modelName, float x, float y, float yaw);
+
+  /// \brief Set the complete state of a model
+  public: void SetState(const char *modelName, const Pose &modelPose, 
+              const Vec3 &linearVel, const Vec3 &angularVel, 
+              const Vec3 &linearAccel, const Vec3 &angularAccel );
+
   /// Pointer to the simulation data
   public: SimulationData *data;
 };


Property changes on: code/branches/federation/gazebo/player_cfgs/epuck.cfg
___________________________________________________________________
Deleted: svn:mergeinfo
   - 


Property changes on: code/branches/federation/gazebo/player_cfgs/epuck_single.cfg
___________________________________________________________________
Deleted: svn:mergeinfo
   - 

Modified: code/branches/federation/gazebo/server/Model.cc
===================================================================
--- code/branches/federation/gazebo/server/Model.cc	2009-03-08 01:54:38 UTC (rev 7376)
+++ code/branches/federation/gazebo/server/Model.cc	2009-03-08 01:56:07 UTC (rev 7377)
@@ -518,8 +518,37 @@
     body->SetAngularVel( vel );
   }
 }
- 
+
 ////////////////////////////////////////////////////////////////////////////////
+/// Set the linear acceleration of the model
+void Model::SetLinearAccel( const Vector3 &accel )
+{
+  Body *body;
+  std::map<std::string, Body* >::iterator iter;
+
+  for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++)
+  {
+    body = iter->second;
+
+    body->SetLinearAccel( accel );
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the angular acceleration of the model
+void Model::SetAngularAccel( const Vector3 &accel )
+{
+  Body *body;
+  std::map<std::string, Body* >::iterator iter;
+
+  for (iter=this->bodies.begin(); iter!=this->bodies.end(); iter++)
+  {
+    body = iter->second;
+
+    body->SetAngularAccel( accel );
+  }
+}
+////////////////////////////////////////////////////////////////////////////////
 // Get the current pose
 const Pose3d &Model::GetPose() const
 {

Modified: code/branches/federation/gazebo/server/Model.hh
===================================================================
--- code/branches/federation/gazebo/server/Model.hh	2009-03-08 01:54:38 UTC (rev 7376)
+++ code/branches/federation/gazebo/server/Model.hh	2009-03-08 01:56:07 UTC (rev 7377)
@@ -113,6 +113,12 @@
 
     /// \brief Set the angular velocity of the model
     public: void SetAngularVel( const Vector3 &vel );
+
+    /// \brief Set the linear acceleration of the model
+    public: void SetLinearAccel( const Vector3 &vel );
+
+    /// \brief Set the angular acceleration of the model
+    public: void SetAngularAccel( const Vector3 &vel );
  
     /// \brief Get the current pose
     public: const Pose3d &GetPose() const;

Modified: code/branches/federation/gazebo/server/World.cc
===================================================================
--- code/branches/federation/gazebo/server/World.cc	2009-03-08 01:54:38 UTC (rev 7376)
+++ code/branches/federation/gazebo/server/World.cc	2009-03-08 01:56:07 UTC (rev 7377)
@@ -597,19 +597,32 @@
             Vector3 angularVel( req->modelAngularVel.x,
                                 req->modelAngularVel.y,
                                 req->modelAngularVel.z);
+            Vector3 linearAccel( req->modelLinearAccel.x,
+                                 req->modelLinearAccel.y,
+                                 req->modelLinearAccel.z);
+            Vector3 angularAccel( req->modelAngularAccel.x,
+                                  req->modelAngularAccel.y,
+                                  req->modelAngularAccel.z);
 
+
             pose.pos.x = req->modelPose.pos.x;
             pose.pos.y = req->modelPose.pos.y;
             pose.pos.z = req->modelPose.pos.z;
 
+            // The the model's pose
             pose.rot.SetFromEuler(
                 Vector3(req->modelPose.roll, 
                   req->modelPose.pitch,
                   req->modelPose.yaw));
             model->SetPose(pose);
 
+            // Set the model's linear and angular velocity
             model->SetLinearVel(linearVel);
             model->SetAngularVel(angularVel);
+
+            // Set the model's linear and angular acceleration
+            model->SetLinearAccel(linearAccel);
+            model->SetAngularAccel(angularAccel);
           }
           else
           {

Modified: code/branches/federation/gazebo/server/physics/Body.cc
===================================================================
--- code/branches/federation/gazebo/server/physics/Body.cc	2009-03-08 01:54:38 UTC (rev 7376)
+++ code/branches/federation/gazebo/server/physics/Body.cc	2009-03-08 01:56:07 UTC (rev 7377)
@@ -650,12 +650,43 @@
 
   return vel;
 }
+
 ////////////////////////////////////////////////////////////////////////////////
+/// Set the linear acceleration of the body
+void Body::SetLinearAccel(const Vector3 &accel)
+{
+  this->SetForce( accel * this->GetMass());
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the angular acceleration of the body
+Vector3 Body::GetLinearAccel() const
+{
+  return this->GetForce() /  this->GetMass();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the angular acceleration of the body
+void Body::SetAngularAccel(const Vector3 &accel)
+{
+  this->SetTorque( accel * this->GetMass());
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the angular acceleration of the body
+Vector3 Body::GetAngularAccel() const
+{
+  return this->GetTorque() /  this->GetMass();
+}
+
+////////////////////////////////////////////////////////////////////////////////
 /// \brief Set the force applied to the body
 void Body::SetForce(const Vector3 &force)
 {
   if (this->bodyId)
-    dBodySetForce(this->bodyId, force.x, force.y, force.z);
+  {
+    dBodyAddForce(this->bodyId, force.x, force.y, force.z);
+  }
 }
 
 ////////////////////////////////////////////////////////////////////////////////

Modified: code/branches/federation/gazebo/server/physics/Body.hh
===================================================================
--- code/branches/federation/gazebo/server/physics/Body.hh	2009-03-08 01:54:38 UTC (rev 7376)
+++ code/branches/federation/gazebo/server/physics/Body.hh	2009-03-08 01:56:07 UTC (rev 7377)
@@ -129,6 +129,18 @@
     /// \brief Get the angular velocity of the body
     public: Vector3 GetAngularVel() const;
 
+    /// \brief Set the linear acceleration of the body
+    public: void SetLinearAccel(const Vector3 &accel);
+
+    /// \brief Get the linear acceleration of the body
+    public: Vector3 GetLinearAccel() const;
+
+    /// \brief Set the angular acceleration of the body
+    public: void SetAngularAccel(const Vector3 &accel);
+
+    /// \brief Get the angular acceleration of the body
+    public: Vector3 GetAngularAccel() const;
+
     /// \brief Set the force applied to the body
     public: void SetForce(const Vector3 &force);
 
@@ -151,7 +163,7 @@
     public: Model *GetModel() const;
 
     /// \brief Get the mass of the body
-    public: float GetMass() { return mass.mass; }
+    public: float GetMass() const { return mass.mass; }
 
     /// Load a new geom helper function
     /// \param node XMLConfigNode used to load the geom

Modified: code/branches/federation/gazebo/worlds/pioneer2dx.world
===================================================================
--- code/branches/federation/gazebo/worlds/pioneer2dx.world	2009-03-08 01:54:38 UTC (rev 7376)
+++ code/branches/federation/gazebo/worlds/pioneer2dx.world	2009-03-08 01:56:07 UTC (rev 7377)
@@ -96,6 +96,7 @@
   <model:physical name="pioneer2dx_model1">
     <xyz>0 0 0.145</xyz>
     <rpy>0.0 0.0 0.0</rpy>
+    <disableGravity/>
 
     <controller:differential_position2d name="controller1">
       <leftJoint>left_wheel_hinge</leftJoint>


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