Revision: 8756
http://playerstage.svn.sourceforge.net/playerstage/?rev=8756&view=rev
Author: natepak
Date: 2010-06-09 15:28:40 +0000 (Wed, 09 Jun 2010)
Log Message:
-----------
Update the pioneer2 gripper and added more plugins
Modified Paths:
--------------
code/gazebo/branches/simpar/Media/materials/scripts/Gazebo.material
code/gazebo/branches/simpar/plugins/CMakeLists.txt
code/gazebo/branches/simpar/plugins/pioneer_circle.cc
code/gazebo/branches/simpar/plugins/pioneer_line.cc
code/gazebo/branches/simpar/server/physics/Joint.cc
code/gazebo/branches/simpar/server/physics/ode/ODEJoint.cc
code/gazebo/branches/simpar/server/rendering/OgreVisual.cc
code/gazebo/branches/simpar/worlds/empty.world
code/gazebo/branches/simpar/worlds/models/pioneer2gripper.model
code/gazebo/branches/simpar/worlds/pioneer2dx_gripper.world
Added Paths:
-----------
code/gazebo/branches/simpar/plugins/ball_drop.cc
code/gazebo/branches/simpar/plugins/pioneer_gripper.cc
Modified: code/gazebo/branches/simpar/Media/materials/scripts/Gazebo.material
===================================================================
--- code/gazebo/branches/simpar/Media/materials/scripts/Gazebo.material
2010-06-09 15:26:38 UTC (rev 8755)
+++ code/gazebo/branches/simpar/Media/materials/scripts/Gazebo.material
2010-06-09 15:28:40 UTC (rev 8756)
@@ -103,8 +103,6 @@
{
ambient 0.000000 0.000000 0.000000 1.000000
diffuse 0.000000 0.000000 0.000000 1.000000
- specular 0.000000 0.000000 0.000000 1.000000
- emissive 0.000000 0.000000 0.000000 0.000000
lighting on
shading phong
}
Modified: code/gazebo/branches/simpar/plugins/CMakeLists.txt
===================================================================
--- code/gazebo/branches/simpar/plugins/CMakeLists.txt 2010-06-09 15:26:38 UTC
(rev 8755)
+++ code/gazebo/branches/simpar/plugins/CMakeLists.txt 2010-06-09 15:28:40 UTC
(rev 8756)
@@ -16,7 +16,10 @@
endif (PKG_CONFIG_FOUND)
set (plugins pioneer_line
- pioneer_circle)
+ pioneer_circle
+ pioneer_gripper
+ ball_drop
+ )
foreach (src ${plugins})
Added: code/gazebo/branches/simpar/plugins/ball_drop.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/ball_drop.cc
(rev 0)
+++ code/gazebo/branches/simpar/plugins/ball_drop.cc 2010-06-09 15:28:40 UTC
(rev 8756)
@@ -0,0 +1,225 @@
+#include <boost/bind.hpp>
+
+#include <gazebo/gazeboserver.hh>
+
+#define LOG true
+
+namespace gazebo
+{
+
+ class BallDrop : public Handler
+ {
+ public: BallDrop() : Handler()
+ {
+ this->sphere = NULL;
+ this->model_name = "sphere";
+ //this->SpawnBall(0,0,5);
+
+ for (double i=0.001; i > 1e-5; i*=0.5)
+ this->stepTimes.push_back(i);
+
+ for (unsigned int i=10; i<=100; i+=10)
+ this->stepIters.push_back(i);
+
+ this->stepTypes.push_back("robust");
+ this->stepTypes.push_back("world");
+ this->stepTypes.push_back("quick");
+
+ this->stepTypesIter = this->stepTypes.begin();
+ this->stepTimesIter = this->stepTimes.begin();
+ this->stepItersIter = this->stepIters.begin();
+
+ this->path = std::string("/home/nate/work/simpar/data/ball_drop/");
+ system( (std::string("mkdir -p ") + this->path).c_str() );
+
+ if (LOG)
+ {
+ this->indexFile = fopen(std::string(this->path + "index.txt").c_str(),
"w");
+ fprintf(this->indexFile, "# index step_type step_time
step_iterations\n");
+ }
+ }
+
+ public: ~BallDrop()
+ {
+ if (LOG)
+ fclose(this->indexFile);
+
+ World::Instance()->DisconnectWorldUpdateStartSignal(
+ boost::bind(&BallDrop::UpdateCB, this));
+
+ /*for (unsigned int i=0; i < this->sphere->GetChildCount(); i++)
+ {
+ Body *body = dynamic_cast<Body*>(this->sphere->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));
+ }
+ }
+ this->sphere->Reset();
+ */
+ }
+
+ public: void SpawnBall(double x, double y, double z=1)
+ {
+ std::ostringstream model;
+
+ model << "<?xml version='1.0'?> <gazebo:world
xmlns:xi='http://www.w3.org/2001/XInclude'
xmlns:gazebo='http://playerstage.sourceforge.net/gazebo/xmlschema/#gz'
xmlns:model='http://playerstage.sourceforge.net/gazebo/xmlschema/#model'
xmlns:sensor='http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor'
xmlns:body='http://playerstage.sourceforge.net/gazebo/xmlschema/#body'
xmlns:geom='http://playerstage.sourceforge.net/gazebo/xmlschema/#geom'
xmlns:joint='http://playerstage.sourceforge.net/gazebo/xmlschema/#joint'
xmlns:interface='http://playerstage.sourceforge.net/gazebo/xmlschema/#interface'
xmlns:rendering='http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering'
xmlns:renderable='http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable'
xmlns:controller='http://playerstage.sourceforge.net/gazebo/xmlschema/#controller'
xmlns:physics='http://playerstage.sourceforge.net/gazebo/xmlschema/#physics'
>";
+
+ model << "<model:physical name='" << this->model_name << "'>";
+ model << " <xyz>" << x << " " << y << " " << z << "</xyz>";
+ model << " <rpy>0 0 0</rpy>";
+ model << " <body:sphere name='body'>";
+ model << " <geom:sphere name='geom'>";
+ model << " <size>0.25</size>";
+ model << " <mass>1</mass>";
+ model << " <kp>100000000.0</kp>";
+ model << " <kd>1.0</kd>";
+ model << " <bounce>0</bounce>";
+ model << " <bounceVel>100000</bounceVel>";
+ model << " <slip1>0.01</slip1>";
+ model << " <slip2>0.01</slip2>";
+ model << " <visual>";
+ model << " <size>0.5 0.5 0.5</size>";
+ model << " <mesh>unit_sphere</mesh>";
+ model << " <material>Gazebo/Rocky</material>";
+ model << " </visual>";
+ model << " </geom:sphere>";
+ model << " </body:sphere>";
+ model << "</model:physical>";
+
+ model << "</gazebo:world>";
+
+ World::Instance()->InsertEntity( model.str() );
+ }
+
+ public: void Load()
+ {
+ this->physics = World::Instance()->GetPhysicsEngine();
+ this->count = 0;
+ this->prev_z = 0;
+
+ World::Instance()->ConnectWorldUpdateStartSignal(
+ boost::bind(&BallDrop::UpdateCB, this));
+
+ }
+
+ public: void UpdateCB()
+ {
+ /*if (this->sphere == NULL)
+ {
+ this->sphere =
(Model*)World::Instance()->GetEntityByName(this->model_name);
+ if (this->sphere)
+ {
+ this->sphere->GetVisualNode()->SetRibbonTrail(true);
+
+ this->prevTime = Simulator::Instance()->GetRealTime();
+
+ if (LOG)
+ Logger::Instance()->AddLog("sphere","/tmp/sphere.log");
+
+ World::Instance()->ConnectWorldUpdateStartSignal(
+ boost::bind(&BallDrop::UpdateCB, this));
+
+ std::cout << "Type[" << *this->stepTypesIter << "] "
+ << "Time[" << *this->stepTimesIter << "] "
+ << "Iters[" << *this->stepItersIter << "]\n";
+ }
+ }
+ else
+ {
+ Time realTime = Simulator::Instance()->GetRealTime();
+ Pose3d pose = this->sphere->GetWorldPose();
+
+ if ( fabs(prev_z - pose.pos.z) >= 1e-5 )
+ this->prevTime = realTime;
+
+ this->prev_z = pose.pos.z;
+
+ if (realTime - this->prevTime > 10.0)
+ {
+ */
+ this->UpdateJob();
+ //}
+ //}
+ }
+
+ public: void UpdateJob()
+ {
+ if (LOG)
+ {
+ // Logger::Instance()->RemoveLog("sphere");
+
+ std::string mv_cmd = std::string("mv /tmp/sphere.log ") + this->path +
+ "ball_drop_" + boost::lexical_cast<std::string>(this->count) +
+ ".data";
+
+ fprintf(this->indexFile,"%d %s %f %d\n",this->count,
+ (*this->stepTypesIter).c_str(), *this->stepTimesIter,
+ *this->stepItersIter);
+
+ //system(mv_cmd.c_str());
+ }
+
+ this->stepItersIter++;
+ if (this->stepItersIter == this->stepIters.end())
+ {
+ this->stepItersIter = this->stepIters.begin();
+ this->stepTimesIter++;
+ if (this->stepTimesIter == this->stepTimes.end())
+ {
+ this->stepTimesIter = this->stepTimes.begin();
+ this->stepTypesIter++;
+ if (this->stepTypesIter == this->stepTypes.end())
+ {
+ Simulator::Instance()->RemoveHandler(this->name);
+ return;
+ }
+ }
+ }
+
+ /*this->physics->SetStepType( *this->stepTypesIter );
+ this->physics->SetStepTime( *this->stepTimesIter );
+ this->physics->SetSORPGSIters( *this->stepItersIter );
+ */
+
+ this->count++;
+ std::cout << "Type[" << *this->stepTypesIter << "] "
+ << "Time[" << *this->stepTimesIter << "] "
+ << "Iters[" << *this->stepItersIter << "] "
+ << "Count[" << this->count << "]\n";
+
+ //this->sphere->Reset();
+ this->prevTime = Simulator::Instance()->GetRealTime();
+
+ /*if (LOG)
+ Logger::Instance()->AddLog("sphere","/tmp/sphere.log");
+ */
+ }
+
+ private: std::vector<unsigned int> stepIters;
+ private: std::vector<unsigned int>::iterator stepItersIter;
+
+ private: std::vector<double> stepTimes;
+ private: std::vector<double>::iterator stepTimesIter;
+
+ private: std::vector<std::string> stepTypes;
+ private: std::vector<std::string>::iterator stepTypesIter;
+ private: std::string path;
+
+ private: Model *sphere;
+
+ private: FILE *indexFile;
+ private: Time prevTime;
+ private: PhysicsEngine *physics;
+ private: unsigned int count;
+
+ private: double prev_z;
+
+ private: std::string model_name;
+ };
+
+ GZ_REGISTER_HANDLER("BallDrop", BallDrop)
+}
Modified: code/gazebo/branches/simpar/plugins/pioneer_circle.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_circle.cc 2010-06-09
15:26:38 UTC (rev 8755)
+++ code/gazebo/branches/simpar/plugins/pioneer_circle.cc 2010-06-09
15:28:40 UTC (rev 8756)
@@ -2,7 +2,7 @@
#include <gazebo/gazeboserver.hh>
-#define LOG false
+#define LOG true
namespace gazebo
{
@@ -72,7 +72,7 @@
this->physics = World::Instance()->GetPhysicsEngine();
- this->prevTime = Simulator::Instance()->GetRealTime();
+ this->prevTime = Simulator::Instance()->GetSimTime();
this->count = 0;
this->circleCount = 0;
@@ -92,7 +92,7 @@
public: void UpdateCB()
{
- Time realTime = Simulator::Instance()->GetRealTime();
+ Time simTime = Simulator::Instance()->GetSimTime();
Pose3d pose = this->robot->GetWorldPose();
if (this->circleCount <= 1)
@@ -103,9 +103,9 @@
if (this->rightWheel->GetRelativeAngularVel().z > 2.1)
this->rightWheel->SetTorque(Vector3(0.0, 0.0, -0.2));
- if (realTime - this->prevTime > 5.0 && fabs(pose.pos.x) < 0.1 )
+ if (simTime - this->prevTime > 10.0 && fabs(pose.pos.x) < 0.1 )
{
- this->prevTime = realTime;
+ this->prevTime = simTime;
this->circleCount++;
}
}
@@ -159,7 +159,7 @@
<< "Count[" << this->count << "]\n";
this->robot->Reset();
- this->prevTime = Simulator::Instance()->GetRealTime();
+ this->prevTime = Simulator::Instance()->GetSimTime();
if (LOG)
Logger::Instance()->AddLog("pioneer","/tmp/pioneer.log");
Added: code/gazebo/branches/simpar/plugins/pioneer_gripper.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_gripper.cc
(rev 0)
+++ code/gazebo/branches/simpar/plugins/pioneer_gripper.cc 2010-06-09
15:28:40 UTC (rev 8756)
@@ -0,0 +1,268 @@
+#include <stdlib.h>
+#include <boost/bind.hpp>
+#include <gazebo/gazeboserver.hh>
+
+#define LOG false
+
+namespace gazebo
+{
+
+ class PioneerGripper : public Handler
+ {
+ public: PioneerGripper() : Handler()
+ {
+ //for (double i=0.001; i > 1e-5; i*=0.5)
+ //this->stepTimes.push_back(i);
+ this->stepTimes.push_back(0.00001);
+
+ for (unsigned int i=10; i<=100; i+=10)
+ this->stepIters.push_back(i);
+
+ this->stepTypes.push_back("quick");
+ this->stepTypes.push_back("robust");
+ this->stepTypes.push_back("world");
+
+ this->stepTypesIter = this->stepTypes.begin();
+ this->stepTimesIter = this->stepTimes.begin();
+ this->stepItersIter = this->stepIters.begin();
+
+ this->path = std::string("/home/nate/work/simpar/data/pioneer_gripper/");
+ system( (std::string("mkdir -p ") + this->path).c_str() );
+
+ this->count = 0;
+
+ if (LOG)
+ {
+ this->indexFile = fopen(std::string(this->path + "index.txt").c_str(),
"w");
+ fprintf(this->indexFile, "# index step_type step_time
step_iterations\n");
+ }
+ }
+
+ public: ~PioneerGripper()
+ {
+ if (LOG)
+ fclose(this->indexFile);
+
+ World::Instance()->DisconnectWorldUpdateStartSignal(
+ boost::bind(&PioneerGripper::UpdateCB, 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));
+ }
+ }
+ this->robot->Reset();
+ }
+
+ public: void Load()
+ {
+ std::string model_name = "pioneer_gripper";
+ this->robot = (Model*)World::Instance()->GetEntityByName(model_name);
+
+ if (this->robot)
+ {
+ this->leftPaddle = (Body*)this->robot->GetChild("left_paddle_body");
+ this->rightPaddle = (Body*)this->robot->GetChild("right_paddle_body");
+ this->liftBody = (Body*)this->robot->GetChild("lift_body");
+
+ World::Instance()->ConnectWorldUpdateStartSignal(
+ boost::bind(&PioneerGripper::UpdateCB, this));
+
+ this->prevTime = Simulator::Instance()->GetSimTime();
+
+ this->physics = World::Instance()->GetPhysicsEngine();
+
+ if (LOG)
+ Logger::Instance()->AddLog("pioneer","/tmp/pioneer.log");
+
+ /*this->physics->SetStepTime( *this->stepTimesIter );
+ this->physics->SetStepType( *this->stepTypesIter );
+ this->physics->SetSORPGSIters( *this->stepItersIter );
+ */
+ std::cout << "Type[" << *this->stepTypesIter << "] "
+ << "Time[" << *this->stepTimesIter << "] "
+ << "Iters[" << *this->stepItersIter << "]\n";
+ }
+ else
+ std::cerr << "Unable to find model[" << model_name << "]\n";
+
+ this->spawned = false; this->leftForce = 0;
+ this->rightForce = 0;
+ }
+
+ public: void SpawnBox(double x, double y, double z=1)
+ {
+ std::ostringstream model;
+
+ model << "<?xml version='1.0'?> <gazebo:world
xmlns:xi='http://www.w3.org/2001/XInclude'
xmlns:gazebo='http://playerstage.sourceforge.net/gazebo/xmlschema/#gz'
xmlns:model='http://playerstage.sourceforge.net/gazebo/xmlschema/#model'
xmlns:sensor='http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor'
xmlns:body='http://playerstage.sourceforge.net/gazebo/xmlschema/#body'
xmlns:geom='http://playerstage.sourceforge.net/gazebo/xmlschema/#geom'
xmlns:joint='http://playerstage.sourceforge.net/gazebo/xmlschema/#joint'
xmlns:interface='http://playerstage.sourceforge.net/gazebo/xmlschema/#interface'
xmlns:rendering='http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering'
xmlns:renderable='http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable'
xmlns:controller='http://playerstage.sourceforge.net/gazebo/xmlschema/#controller'
xmlns:physics='http://playerstage.sourceforge.net/gazebo/xmlschema/#physics'
>";
+
+ model << "<model:physical name='custom_box'>";
+ model << " <xyz>" << x << " " << y << " " << z << "</xyz>";
+ model << " <rpy>0 0 0</rpy>";
+ model << " <body:box name='body'>";
+ model << " <geom:box name='geom'>";
+ model << " <size>0.1 0.10 0.2</size>";
+ model << " <mass>0.01</mass>";
+ model << " <kp>100000000.0</kp>";
+ model << " <kd>1.0</kd>";
+ model << " <bounce>0</bounce>";
+ model << " <bounceVel>100000</bounceVel>";
+ model << " <slip1>0.01</slip1>";
+ model << " <slip2>0.01</slip2>";
+ model << " <visual>";
+ model << " <size>0.1 0.1 0.2</size>";
+ model << " <mesh>unit_box</mesh>";
+ model << " <material>Gazebo/Rocky</material>";
+ model << " </visual>";
+ model << " </geom:box>";
+ model << " </body:box>";
+ model << "</model:physical>";
+
+ model << "</gazebo:world>";
+
+ World::Instance()->InsertEntity( model.str() );
+
+ this->spawned = true;
+ }
+
+ public: void Open()
+ {
+ std::cout << "Open ";
+ this->leftForce = 0.01;
+ this->rightForce = -0.01;
+ }
+
+ public: void Close()
+ {
+ std::cout << "Close ";
+ this->leftForce = -1.00;
+ this->rightForce = 1.00;
+ }
+
+ public: void Lift()
+ {
+ std::cout << "Lift ";
+ this->liftBody->SetForce(Vector3(0,0,5.0));
+ }
+
+ public: bool IsOpen()
+ {
+ return this->leftPaddle->GetWorldPose().pos.y >= 0.13 &&
+ this->rightPaddle->GetWorldPose().pos.y <= -0.13;
+ }
+
+ public: bool IsClosed()
+ {
+ double diff = this->leftPaddle->GetWorldPose().pos.y -
+ this->rightPaddle->GetWorldPose().pos.y;
+ std::cout << "DIFF[" << diff << "]\n";
+
+ return fabs(diff) < 0.130;
+ //return this->leftPaddle->GetWorldPose().pos.y <= 0.07 &&
+ //this->rightPaddle->GetWorldPose().pos.y >= -0.07;
+ }
+
+ public: void UpdateCB()
+ {
+ Time simTime = Simulator::Instance()->GetSimTime();
+
+ this->leftPaddle->SetForce(Vector3(0.0,this->leftForce,0.0));
+ this->rightPaddle->SetForce(Vector3(0.0,this->rightForce,0));
+
+ if (this->IsOpen() && !this->spawned)
+ this->SpawnBox(0.15, 0, 0.1);
+
+ if (World::Instance()->GetEntityByName("custom_box"))
+ {
+ this->Close();
+ if (this->IsClosed())
+ this->Lift();
+ }
+ else
+ this->Open();
+
+ std::cout << "\n";
+ }
+
+
+ public: void UpdateJob()
+ {
+ if (LOG)
+ {
+ Logger::Instance()->RemoveLog("pioneer");
+
+ std::string mv_cmd = std::string("mv /tmp/pioneer.log ") + this->path
+
+ "pioneer_line_" + boost::lexical_cast<std::string>(this->count) +
+ ".data";
+
+ fprintf(this->indexFile,"%d %s %f %d\n",this->count,
+ (*this->stepTypesIter).c_str(), *this->stepTimesIter,
+ *this->stepItersIter);
+
+ system(mv_cmd.c_str());
+ }
+
+ this->stepItersIter++;
+ if (this->stepItersIter == this->stepIters.end())
+ {
+ this->stepItersIter = this->stepIters.begin();
+ this->stepTimesIter++;
+ if (this->stepTimesIter == this->stepTimes.end())
+ {
+ this->stepTimesIter = this->stepTimes.begin();
+ this->stepTypesIter++;
+ if (this->stepTypesIter == this->stepTypes.end())
+ {
+ Simulator::Instance()->RemoveHandler(this->name);
+ return;
+ }
+ }
+ }
+
+ this->physics->SetStepType( *this->stepTypesIter );
+ this->physics->SetStepTime( *this->stepTimesIter );
+ this->physics->SetSORPGSIters( *this->stepItersIter );
+
+ this->count++;
+ std::cout << "Type[" << *this->stepTypesIter << "] "
+ << "Time[" << *this->stepTimesIter << "] "
+ << "Iters[" << *this->stepItersIter << "] "
+ << "Count[" << this->count << "]\n";
+
+ this->robot->Reset();
+ this->prevTime = Simulator::Instance()->GetSimTime();
+
+ if (LOG)
+ Logger::Instance()->AddLog("pioneer","/tmp/pioneer.log");
+ }
+
+ private: std::vector<unsigned int> stepIters;
+ private: std::vector<unsigned int>::iterator stepItersIter;
+
+ private: std::vector<double> stepTimes;
+ private: std::vector<double>::iterator stepTimesIter;
+
+ private: std::vector<std::string> stepTypes;
+ private: std::vector<std::string>::iterator stepTypesIter;
+ private: std::string path;
+ private: unsigned int count;
+
+ private: Model *robot;
+ private: Body *leftPaddle, *rightPaddle, *liftBody;
+ private: FILE *indexFile;
+ private: Time prevTime;
+ private: PhysicsEngine *physics;
+
+ private: bool spawned;
+
+ private: double leftForce, rightForce;
+ };
+
+ GZ_REGISTER_HANDLER("PioneerGripper", PioneerGripper)
+}
Modified: code/gazebo/branches/simpar/plugins/pioneer_line.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_line.cc 2010-06-09 15:26:38 UTC
(rev 8755)
+++ code/gazebo/branches/simpar/plugins/pioneer_line.cc 2010-06-09 15:28:40 UTC
(rev 8756)
@@ -75,7 +75,7 @@
World::Instance()->ConnectWorldUpdateStartSignal(
boost::bind(&PioneerLine::UpdateCB, this));
- this->prevTime = Simulator::Instance()->GetRealTime();
+ this->prevTime = Simulator::Instance()->GetSimTime();
this->physics = World::Instance()->GetPhysicsEngine();
@@ -95,14 +95,14 @@
public: void UpdateCB()
{
- Time realTime = Simulator::Instance()->GetRealTime();
+ Time simTime = Simulator::Instance()->GetSimTime();
- if (realTime - prevTime < Time(0,100000000))
+ if (simTime - prevTime < Time(0,100000000))
{
this->leftWheel->SetTorque(Vector3(0.0,0.0,.1));
this->rightWheel->SetTorque(Vector3(0.0,0.0,.1));
}
- else if (realTime - prevTime >= Time(20,0))
+ else if (simTime - prevTime >= Time(20,0))
this->UpdateJob();
}
@@ -152,7 +152,7 @@
<< "Count[" << this->count << "]\n";
this->robot->Reset();
- this->prevTime = Simulator::Instance()->GetRealTime();
+ this->prevTime = Simulator::Instance()->GetSimTime();
if (LOG)
Logger::Instance()->AddLog("pioneer","/tmp/pioneer.log");
Modified: code/gazebo/branches/simpar/server/physics/Joint.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/Joint.cc 2010-06-09 15:26:38 UTC
(rev 8755)
+++ code/gazebo/branches/simpar/server/physics/Joint.cc 2010-06-09 15:28:40 UTC
(rev 8756)
@@ -237,6 +237,8 @@
//TODO: Evaluate impact of this code on performance
if (this->visual && this->visual->GetVisible())
{
+ if (this->GetName() == "left_paddle_joint")
+ std::cout << "Joint[" << this->GetName() << "] Angle[" <<
this->GetAngle(0) << "]\n";
this->anchorPos = (Pose3d(**(this->anchorOffsetP),Quatern()) +
this->anchorBody->GetWorldPose()).pos;
Modified: code/gazebo/branches/simpar/server/physics/ode/ODEJoint.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEJoint.cc 2010-06-09
15:26:38 UTC (rev 8755)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEJoint.cc 2010-06-09
15:28:40 UTC (rev 8756)
@@ -201,7 +201,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());
@@ -210,14 +210,13 @@
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());
@@ -226,7 +225,6 @@
case 2:
this->SetParam(dParamLoStop3, angle.GetAsRadian());
};
- */
}
////////////////////////////////////////////////////////////////////////////////
@@ -326,13 +324,12 @@
case VEL:
this->SetParam(dParamVel, value);
break;
- /*case HI_STOP:
+ case HI_STOP:
this->SetParam(dParamHiStop, value);
break;
case LO_STOP:
this->SetParam(dParamLoStop, value);
break;
- */
default:
gzerr(0) << "Unable to handle joint attribute[" << attr << "]\n";
break;
Modified: code/gazebo/branches/simpar/server/rendering/OgreVisual.cc
===================================================================
--- code/gazebo/branches/simpar/server/rendering/OgreVisual.cc 2010-06-09
15:26:38 UTC (rev 8755)
+++ code/gazebo/branches/simpar/server/rendering/OgreVisual.cc 2010-06-09
15:28:40 UTC (rev 8756)
@@ -600,64 +600,65 @@
if (!Simulator::Instance()->GetRenderEngineEnabled())
return;
-
- unsigned short i = 0, j=0;
- Ogre::ColourValue sc, dc;
- Ogre::Technique *t;
-
- this->transparency = std::min(std::max(trans, (float)0.0), (float)1.0);
-
- if (this->myMaterial.isNull())
+ double transparency = std::min(std::max(trans, (float)0.0), (float)1.0);
+ for (unsigned int i=0; i < this->sceneNode->numAttachedObjects(); i++)
{
- gzmsg(0) << "The visual can't set transparency for this geom (" << /*geom
name? << */ ") without a material"
- << " sceneNode(" << this->sceneNode->getName() << ")\n";
- return;
- }
+ Ogre::Entity *entity = NULL;
+ Ogre::SimpleRenderable *simple = NULL;
+ Ogre::MovableObject *obj = this->sceneNode->getAttachedObject(i);
- Ogre::Material::TechniqueIterator techniqueIt =
this->myMaterial->getTechniqueIterator();
+ entity = dynamic_cast<Ogre::Entity*>(obj);
+ //simple = dynamic_cast<Ogre::SimpleRenderable*>(obj);
+ if (!entity)
+ continue;
- while ( techniqueIt.hasMoreElements() )
- {
- t = techniqueIt.getNext ();
- Ogre::Technique::PassIterator passIt = t->getPassIterator ();
-
- j = 0;
-
- while (passIt.hasMoreElements ())
+ for (unsigned int j=0; j < entity->getNumSubEntities(); j++)
{
- sc = this->myMaterial->getTechnique(i)->getPass(j)->getDiffuse();
+ Ogre::SubEntity *subEntity = entity->getSubEntity(j);
+ Ogre::MaterialPtr material = subEntity->getMaterial();
+ Ogre::Material::TechniqueIterator techniqueIt =
material->getTechniqueIterator();
- if (this->transparency >0.0)
- passIt.peekNext ()->setDepthWriteEnabled (false);
- else
- passIt.peekNext ()->setDepthWriteEnabled (true);
+ unsigned int techniqueCount, passCount;
+ Ogre::Technique *technique;
+ Ogre::Pass *pass;
+ Ogre::ColourValue sc, dc;
- switch (this->sceneBlendType)
+ for (techniqueCount = 0; techniqueCount < material->getNumTechniques();
+ techniqueCount++)
{
- case Ogre::SBT_ADD:
- dc = sc;
- dc.r -= sc.r * this->transparency;
- dc.g -= sc.g * this->transparency;
- dc.b -= sc.b * this->transparency;
- passIt.peekNext()->setAmbient(Ogre::ColourValue::Black);
- break;
+ technique = material->getTechnique(techniqueCount);
- case Ogre::SBT_TRANSPARENT_ALPHA:
- default:
- dc = sc;
- //dc.a = sc.a * (1.0f - this->transparency);
- dc.a = (1.0f - this->transparency);
-
passIt.peekNext()->setAmbient(this->myMaterial->getTechnique(i)->getPass(j)->getAmbient());
- break;
- }
- passIt.peekNext ()->setDiffuse (dc);
+ for (passCount=0; passCount < technique->getNumPasses(); passCount++)
+ {
+ pass = technique->getPass(passCount);
+ sc = pass->getDiffuse();
- passIt.moveNext ();
+ if (transparency > 0.0)
+ pass->setDepthWriteEnabled(false);
+ else
+ pass->setDepthWriteEnabled(true);
- ++j;
+ switch (this->sceneBlendType)
+ {
+ case Ogre::SBT_ADD:
+ dc = sc;
+ dc.r -= sc.r * transparency;
+ dc.g -= sc.g * transparency;
+ dc.b -= sc.b * transparency;
+ pass->setAmbient(Ogre::ColourValue::Black);
+ break;
+
+ case Ogre::SBT_TRANSPARENT_ALPHA:
+ default:
+ dc = sc;
+ dc.a = (1.0f - transparency);
+ pass->setAmbient(pass->getAmbient());
+ break;
+ }
+ pass->setDiffuse(dc);
+ }
+ }
}
-
- ++i;
}
}
Modified: code/gazebo/branches/simpar/worlds/empty.world
===================================================================
--- code/gazebo/branches/simpar/worlds/empty.world 2010-06-09 15:26:38 UTC
(rev 8755)
+++ code/gazebo/branches/simpar/worlds/empty.world 2010-06-09 15:28:40 UTC
(rev 8756)
@@ -21,7 +21,7 @@
<erp>0.2</erp>
<stepType>robust</stepType>
- <stepIters>100</stepIters>
+ <stepIters>10</stepIters>
<stepW>1.3</stepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.0</contactSurfaceLayer>
Modified: code/gazebo/branches/simpar/worlds/models/pioneer2gripper.model
===================================================================
--- code/gazebo/branches/simpar/worlds/models/pioneer2gripper.model
2010-06-09 15:26:38 UTC (rev 8755)
+++ code/gazebo/branches/simpar/worlds/models/pioneer2gripper.model
2010-06-09 15:28:40 UTC (rev 8756)
@@ -18,12 +18,12 @@
<canonicalBody>gripper_base_body</canonicalBody>
<body:box name="gripper_base_body">
- <xyz>0.0 0 0.0</xyz>
+ <xyz>0.0 0 0.065</xyz>
<geom:box name="gripper_base_geom">
<xyz>0 0 0</xyz>
<size>0.053 0.123 0.130</size>
- <mass>0.01</mass>
+ <mass>0.1</mass>
<visual>
<size>0.053 0.123 0.130</size>
@@ -33,13 +33,13 @@
</geom:box>
</body:box>
-<body:box name="gripper_lift_body">
+<body:box name="lift_body">
<xyz>0.061 0 0</xyz>
- <geom:box name="gripper_lift_body">
+ <geom:box name="lift_geom">
<xyz>0 0 0</xyz>
<size>0.069 0.313 0.063</size>
- <mass>0</mass>
+ <mass>0.1</mass>
<visual>
<size>0.069 0.313 0.063</size>
@@ -49,35 +49,36 @@
</geom:box>
</body:box>
-<body:box name="gripper_left_paddle_body">
- <xyz>0.140 0.0125 0</xyz>
- <geom:box name="gripper_left_paddle_geom">
+<body:box name="left_paddle_body">
+ <xyz>0.145 0.0135 0</xyz>
+ <geom:box name="left_paddle_geom">
<xyz>0 0 0</xyz>
<size>0.095 0.025 0.039</size>
- <mass>0</mass>
+ <mass>0.1</mass>
<visual>
<size>0.095 0.025 0.039</size>
<mesh>unit_box</mesh>
- <material>Gazebo/Black</material>
+ <material>Gazebo/Blue</material>
</visual>
</geom:box>
</body:box>
-<body:box name="gripper_right_paddle_body">
- <xyz>0.140 -0.0125 0</xyz>
- <geom:box name="gripper_right_paddle_geom">
+<body:box name="right_paddle_body">
+ <xyz>0.145 -0.0135 0</xyz>
+ <geom:box name="right_paddle_geom">
<xyz>0 0 0</xyz>
<size>0.095 0.025 0.039</size>
- <mass>0</mass>
+ <mass>0.1</mass>
<visual>
<size>0.095 0.025 0.039</size>
<mesh>unit_box</mesh>
- <material>Gazebo/Black</material>
+ <material>Gazebo/Red</material>
</visual>
</geom:box>
+ <!--
<sensor:ray name="breakbeam1">
<rayCount>1</rayCount>
<origin>0 0 0</origin>
@@ -85,44 +86,64 @@
<minRange>0.01</minRange>
<maxRange>0.5</maxRange>
</sensor:ray>
+ -->
</body:box>
+<joint:hinge name="fixed">
+ <body1>gripper_base_body</body1>
+ <body2>world</body2>
+ <anchor>gripper_base_body</anchor>
+ <anchorOffset>0 0 -0.130</anchorOffset>
+ <axis>0 0 1</axis>
+ <lowStop>0</lowStop>
+ <highStop>0</highStop>
+ <erp>0.4</erp>
+ <cfm>0.01</cfm>
+</joint:hinge>
+
<joint:slider name="lift_slider_joint">
<body1>gripper_base_body</body1>
- <body2>gripper_lift_body</body2>
- <axis>0 0 -1</axis>
- <lowStop>-0.05</lowStop>
- <highStop>0.05</highStop>
+ <body2>lift_body</body2>
+ <anchor>gripper_base_body</anchor>
+ <axis>0 0 1</axis>
+ <lowStop>-0.1</lowStop>
+ <highStop>0.1</highStop>
<erp>0.3</erp>
<cfm>10e-5</cfm>
</joint:slider>
<joint:slider name="left_paddle_joint">
- <body1>gripper_lift_body</body1>
- <body2>gripper_left_paddle_body</body2>
+ <body1>lift_body</body1>
+ <body2>left_paddle_body</body2>
+ <anchor>lift_body</anchor>
+ <anchorOffset>0.085 0 0 </anchorOffset>
<axis>0 1 0</axis>
<lowStop>-0.12</lowStop>
<highStop>0.0</highStop>
- <erp>0.3</erp>
+ <erp>0.1</erp>
<cfm>10e-5</cfm>
</joint:slider>
<joint:slider name="right_paddle_joint">
- <body1>gripper_lift_body</body1>
- <body2>gripper_right_paddle_body</body2>
+ <body1>lift_body</body1>
+ <body2>right_paddle_body</body2>
+ <anchor>lift_body</anchor>
+ <anchorOffset>0.085 0 0 </anchorOffset>
<axis>0 1 0</axis>
<lowStop>0.0</lowStop>
<highStop>0.12</highStop>
- <erp>0.3</erp>
+ <erp>0.1</erp>
<cfm>10e-5</cfm>
</joint:slider>
+<!--
<controller:pioneer2_gripper name="pioneer2gripper_controller">
<leftJoint name="left_paddle_joint" force="0.001"/>
<rightJoint name="right_paddle_joint" force="0.001"/>
<liftJoint name="lift_paddle_joint" force="0.1"/>
<interface:gripper name="gripper_iface_0"/>
</controller:pioneer2_gripper>
+-->
</model:physical>
Modified: code/gazebo/branches/simpar/worlds/pioneer2dx_gripper.world
===================================================================
--- code/gazebo/branches/simpar/worlds/pioneer2dx_gripper.world 2010-06-09
15:26:38 UTC (rev 8755)
+++ code/gazebo/branches/simpar/worlds/pioneer2dx_gripper.world 2010-06-09
15:28:40 UTC (rev 8756)
@@ -19,28 +19,29 @@
<verbosity>5</verbosity>
<physics:ode>
- <stepTime>0.03</stepTime>
+ <stepTime>0.0001</stepTime>
<gravity>0 0 -9.8</gravity>
- <cfm>10e-5</cfm>
- <erp>0.8</erp>
- <maxUpdateRate>0</maxUpdateRate>
+ <cfm>0.0000000001</cfm>
+ <erp>0.2</erp>
+
+ <stepType>robust</stepType>
+ <stepIters>10</stepIters>
+ <stepW>1.3</stepW>
+ <contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
+ <contactSurfaceLayer>0.00</contactSurfaceLayer>
+
</physics:ode>
<rendering:gui>
<type>fltk</type>
- <size>640 480</size>
+ <size>800 600</size>
<pos>0 0</pos>
</rendering:gui>
<rendering:ogre>
- <ambient>0.5 0.5 0.5 1.0</ambient>
- <sky>
- <material>Gazebo/CloudySky</material>
- </sky>
- <maxUpdateRate>0</maxUpdateRate>
+ <shadows>true</shadows>
</rendering:ogre>
-
<!-- Ground Plane -->
<model:physical name="plane1_model">
<xyz>0 0 0</xyz>
@@ -50,44 +51,27 @@
<body:plane name="plane1_body">
<geom:plane name="plane1_geom">
<normal>0 0 1</normal>
- <size>2000 2000</size>
- <segments>10 10</segments>
+ <size>100 100</size>
+ <segments>1 1</segments>
<uvTile>100 100</uvTile>
- <material>Gazebo/Grey</material>
+ <material>Gazebo/GrayGrid</material>
</geom:plane>
</body:plane>
</model:physical>
- <!-- The camera -->
- <model:physical name="cam1_model">
- <xyz>0.07 -1.10 0.5</xyz>
- <rpy>0 12 52.5</rpy>
- <static>true</static>
-
- <body:empty name="cam1_body">
- <sensor:camera name="cam1_sensor">
- <imageSize>640 480</imageSize>
- <hfov>60</hfov>
- <nearClip>0.1</nearClip>
- <farClip>100</farClip>
- -->
- </sensor:camera>
- </body:empty>
- </model:physical>
-
-
- <model:physical name="pioneer2dx_model1">
- <xyz>0 0 0.145</xyz>
+ <!--<model:physical name="pioneer2dx_model1">
+ <xyz>0 0 0.148</xyz>
<rpy>0.0 0.0 0.0</rpy>
+ -->
- <model:physical name="gripper">
- <xyz>0.228 0 -0.025</xyz>
-
- <attach>
+ <model:physical name="pioneer_gripper">
+ <!--<xyz>0.228 0 -0.025</xyz>-->
+ <xyz>0 0 0.148</xyz>
+ <!--<attach>
<parentBody>chassis_body</parentBody>
<myBody>gripper_base_body</myBody>
</attach>
-
+ -->
<include embedded = "true">
<xi:include href="models/pioneer2gripper.model"/>
</include>
@@ -97,25 +81,24 @@
The include should be last within a model. All previous statements
will override those in the included file
-->
- <include embedded="true">
+ <!--<include embedded="true">
<xi:include href="models/pioneer2dx.model" />
</include>
- </model:physical>
+ -->
+ <!--</model:physical>-->
<!-- White Directional light -->
<model:renderable name="directional_white">
- <enableGravity>false</enableGravity>
+ <static>false</static>
<light>
<type>directional</type>
- <direction>0 -0.6 -0.4</direction>
- <diffuseColor>1.0 1.0 1.0</diffuseColor>
- <specularColor>0.2 0.2 0.2</specularColor>
+ <direction>0 0.05 -0.9</direction>
+ <diffuseColor>0.7 0.7 0.7</diffuseColor>
+ <specularColor>0.0 0.0 0.0</specularColor>
<range>100</range>
- <!-- Constant(0-1) Linear(0-1) Quadratic -->
- <attenuation>1.0 0.0 0</attenuation>
+ <attenuation>1.0 0.01 0.0001</attenuation>
</light>
</model:renderable>
-
</gazebo:world>
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
ThinkGeek and WIRED's GeekDad team up for the Ultimate
GeekDad Father's Day Giveaway. ONE MASSIVE PRIZE to the
lucky parental unit. See the prize list and enter to win:
http://p.sf.net/sfu/thinkgeek-promo
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
