Revision: 8741
http://playerstage.svn.sourceforge.net/playerstage/?rev=8741&view=rev
Author: natepak
Date: 2010-05-31 21:18:10 +0000 (Mon, 31 May 2010)
Log Message:
-----------
Updates
Modified Paths:
--------------
code/gazebo/branches/simpar/plugins/CMakeLists.txt
code/gazebo/branches/simpar/server/physics/Body.cc
code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc
code/gazebo/branches/simpar/worlds/pioneer2dx.world
Added Paths:
-----------
code/gazebo/branches/simpar/plugins/pioneer_circle.cc
Modified: code/gazebo/branches/simpar/plugins/CMakeLists.txt
===================================================================
--- code/gazebo/branches/simpar/plugins/CMakeLists.txt 2010-05-30 00:37:46 UTC
(rev 8740)
+++ code/gazebo/branches/simpar/plugins/CMakeLists.txt 2010-05-31 21:18:10 UTC
(rev 8741)
@@ -15,8 +15,10 @@
link_directories(${GAZEBO_LIBRARY_DIRS})
endif (PKG_CONFIG_FOUND)
-set (plugins pioneer_spiral )
+set (plugins pioneer_spiral
+ pioneer_circle)
+
foreach (src ${plugins})
set_source_files_properties(${src}.cc PROPERTIES COMPILE_FLAGS
${GAZEBO_CFLAGS})
add_library(${src} SHARED ${src}.cc)
Added: code/gazebo/branches/simpar/plugins/pioneer_circle.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_circle.cc
(rev 0)
+++ code/gazebo/branches/simpar/plugins/pioneer_circle.cc 2010-05-31
21:18:10 UTC (rev 8741)
@@ -0,0 +1,84 @@
+#include <boost/bind.hpp>
+#include <gazebo/gazeboserver.hh>
+
+namespace gazebo
+{
+
+
+ class PioneerCircle : public Handler
+ {
+ public: PioneerCircle() : Handler() {}
+
+ public: ~PioneerCircle()
+ {
+ World::Instance()->DisconnectWorldUpdateStartSignal(
+ boost::bind(&PioneerCircle::UpdateCB, this));
+
+ World::Instance()->DisconnectWorldUpdateEndSignal(
+ boost::bind(&PioneerCircle::UpdateEndCB, this));
+
+ for (unsigned int i=0; i < this->robot->GetChildCount(); i++)
+ {
+ Body *body = dynamic_cast<Body*>(this->robot->GetChild(i));
+ if (body)
+ {
+ body->SetForce(Vector3(0,0,0));
+ body->SetTorque(Vector3(0,0,0));
+ body->SetLinearVel(Vector3(0,0,0));
+ body->SetAngularVel(Vector3(0,0,0));
+ }
+ }
+ }
+
+ public: void Load()
+ {
+
+ std::string model_name = "pioneer";
+ this->robot = (Model*)World::Instance()->GetEntityByName(model_name);
+
+ if (this->robot)
+ {
+ this->robot->GetVisualNode()->SetRibbonTrail(true);
+ this->leftWheel = (Body*)this->robot->GetChild("left_wheel");
+ this->rightWheel = (Body*)this->robot->GetChild("right_wheel");
+
+ World::Instance()->ConnectWorldUpdateStartSignal(
+ boost::bind(&PioneerCircle::UpdateCB, this));
+
+ World::Instance()->ConnectWorldUpdateEndSignal(
+ boost::bind(&PioneerCircle::UpdateEndCB, this));
+ }
+ else
+ std::cerr << "Unable to find model[" << model_name << "]\n";
+ }
+
+ public: void UpdateCB()
+ {
+ if (this->leftWheel->GetAngularVel().y < 0.2)
+ {
+ this->leftWheel->SetTorque(Vector3(0.0, 0.0, 0.08));
+ }
+
+ if (this->rightWheel->GetAngularVel().y > 0.1)
+ this->rightWheel->SetTorque(Vector3(0.0, 0.0, -0.08));
+
+ //this->rightWheel->SetTorque(Vector3(0.0, 0.0, 0.01));
+ //this->rightWheel->SetTorque(Vector3(0.0, 0.0, 0));
+ //this->leftWheel->SetAngularVel(Vector3(0.0, 0.3, 0));
+ //this->rightWheel->SetAngularVel(Vector3(0.0, 0.1, 0));
+ }
+
+ public: void UpdateEndCB()
+ {
+ //std::cout << this->leftWheel->GetTorque() << "\n";
+ //std::cout << this->rightWheel->GetAngularVel() << "\n";
+ }
+
+ private: Model *robot;
+ private: Body *leftWheel;
+ private: Body *rightWheel;
+
+ };
+
+ GZ_REGISTER_HANDLER("PioneerCircle", PioneerCircle)
+}
Modified: code/gazebo/branches/simpar/server/physics/Body.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/Body.cc 2010-05-30 00:37:46 UTC
(rev 8740)
+++ code/gazebo/branches/simpar/server/physics/Body.cc 2010-05-31 21:18:10 UTC
(rev 8741)
@@ -409,10 +409,10 @@
Vector3 avel;
// Apply our linear accel
- this->SetForce(this->linearAccel);
+ //this->SetForce(this->linearAccel);
// Apply our angular accel
- this->SetTorque(this->angularAccel);
+ //this->SetTorque(this->angularAccel);
// FIXME: FIXME: @todo: @todo: race condition on factory-based model
loading!!!!!
// if (this->GetEnabled() != this->enabled)
Modified: code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc 2010-05-30
00:37:46 UTC (rev 8740)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc 2010-05-31
21:18:10 UTC (rev 8741)
@@ -434,7 +434,7 @@
}
////////////////////////////////////////////////////////////////////////////////
-/// Get the velocity of the body
+/// Get the angular velocity of the body
Vector3 ODEBody::GetAngularVel() const
{
Vector3 vel;
@@ -442,14 +442,23 @@
if (this->bodyId)
{
const dReal *dvel;
+ dReal result[3];
this->physicsEngine->LockMutex();
dvel = dBodyGetAngularVel(this->bodyId);
this->physicsEngine->UnlockMutex();
+ dBodyVectorFromWorld(this->bodyId, dvel[0], dvel[1], dvel[2], result);
- vel.x = dvel[0];
- vel.y = dvel[1];
- vel.z = dvel[2];
+ Quatern quat(0, dvel[0], dvel[1], dvel[2]);
+
+ Vector3 vec;
+ quat = this->GetAbsPose().rot * quat;
+ vec.x = quat.x;
+ vec.y = quat.y;
+ vec.z = quat.z;
+
+ printf("ODE[%4.2f %4.2f %4.2f] Me[%4.2f %4.2f %4.2f]\n",
+ result[0], result[1], result[2], vec.x, vec.y, vec.z);
}
return vel;
@@ -497,7 +506,7 @@
{
this->SetEnabled(true);
this->physicsEngine->LockMutex();
- dBodyAddTorque(this->bodyId, torque.x, torque.y, torque.z);
+ dBodyAddRelTorque(this->bodyId, torque.x, torque.y, torque.z);
this->physicsEngine->UnlockMutex();
}
}
Modified: code/gazebo/branches/simpar/worlds/pioneer2dx.world
===================================================================
--- code/gazebo/branches/simpar/worlds/pioneer2dx.world 2010-05-30 00:37:46 UTC
(rev 8740)
+++ code/gazebo/branches/simpar/worlds/pioneer2dx.world 2010-05-31 21:18:10 UTC
(rev 8741)
@@ -90,7 +90,7 @@
</include>
-->
- <model:physical name="pioneer2dx_model">
+ <model:physical name="pioneer">
<xyz>0 0 0.147</xyz>
<rpy>0.0 0.0 0.0</rpy>
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
