Revision: 7373
http://playerstage.svn.sourceforge.net/playerstage/?rev=7373&view=rev
Author: natepak
Date: 2009-03-08 01:32:18 +0000 (Sun, 08 Mar 2009)
Log Message:
-----------
Added convenience functions to libgazebo
Modified Paths:
--------------
code/gazebo/trunk/SConstruct
code/gazebo/trunk/examples/libgazebo/simiface/SConstruct
code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc
code/gazebo/trunk/libgazebo/SConscript
code/gazebo/trunk/libgazebo/gazebo.h
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Model.hh
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Body.hh
Modified: code/gazebo/trunk/SConstruct
===================================================================
--- code/gazebo/trunk/SConstruct 2009-03-08 00:07:03 UTC (rev 7372)
+++ code/gazebo/trunk/SConstruct 2009-03-08 01:32:18 UTC (rev 7373)
@@ -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/gazebo/trunk/examples/libgazebo/simiface/SConstruct
===================================================================
--- code/gazebo/trunk/examples/libgazebo/simiface/SConstruct 2009-03-08
00:07:03 UTC (rev 7372)
+++ code/gazebo/trunk/examples/libgazebo/simiface/SConstruct 2009-03-08
01:32:18 UTC (rev 7373)
@@ -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/gazebo/trunk/examples/libgazebo/simiface/simiface.cc
===================================================================
--- code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2009-03-08
00:07:03 UTC (rev 7372)
+++ code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2009-03-08
01:32:18 UTC (rev 7373)
@@ -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, 0, 0);
+ gazebo::Vec3 angularVel(0, 0, 0);
+ gazebo::Vec3 linearAccel(50, 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/gazebo/trunk/libgazebo/SConscript
===================================================================
--- code/gazebo/trunk/libgazebo/SConscript 2009-03-08 00:07:03 UTC (rev
7372)
+++ code/gazebo/trunk/libgazebo/SConscript 2009-03-08 01:32:18 UTC (rev
7373)
@@ -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)
Modified: code/gazebo/trunk/libgazebo/gazebo.h
===================================================================
--- code/gazebo/trunk/libgazebo/gazebo.h 2009-03-08 00:07:03 UTC (rev
7372)
+++ code/gazebo/trunk/libgazebo/gazebo.h 2009-03-08 01:32:18 UTC (rev
7373)
@@ -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;
};
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2009-03-08 00:07:03 UTC (rev 7372)
+++ code/gazebo/trunk/server/Model.cc 2009-03-08 01:32:18 UTC (rev 7373)
@@ -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/gazebo/trunk/server/Model.hh
===================================================================
--- code/gazebo/trunk/server/Model.hh 2009-03-08 00:07:03 UTC (rev 7372)
+++ code/gazebo/trunk/server/Model.hh 2009-03-08 01:32:18 UTC (rev 7373)
@@ -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/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-03-08 00:07:03 UTC (rev 7372)
+++ code/gazebo/trunk/server/World.cc 2009-03-08 01:32:18 UTC (rev 7373)
@@ -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);
+ //model->SetPose(pose);
- model->SetLinearVel(linearVel);
- model->SetAngularVel(angularVel);
+ // 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/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-03-08 00:07:03 UTC (rev
7372)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-03-08 01:32:18 UTC (rev
7373)
@@ -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/gazebo/trunk/server/physics/Body.hh
===================================================================
--- code/gazebo/trunk/server/physics/Body.hh 2009-03-08 00:07:03 UTC (rev
7372)
+++ code/gazebo/trunk/server/physics/Body.hh 2009-03-08 01:32:18 UTC (rev
7373)
@@ -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
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Open Source Business Conference (OSBC), March 24-25, 2009, San Francisco, CA
-OSBC tackles the biggest issue in open source: Open Sourcing the Enterprise
-Strategies to boost innovation and cut costs with open source participation
-Receive a $600 discount off the registration fee with the source code: SFAD
http://p.sf.net/sfu/XcvMzF8H
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
