Revision: 8761
http://playerstage.svn.sourceforge.net/playerstage/?rev=8761&view=rev
Author: natepak
Date: 2010-06-11 18:12:50 +0000 (Fri, 11 Jun 2010)
Log Message:
-----------
Update the box push plugin
Modified Paths:
--------------
code/gazebo/branches/simpar/plugins/box_push.cc
code/gazebo/branches/simpar/server/Quatern.cc
code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc
code/gazebo/branches/simpar/worlds/3stacks.world
code/gazebo/branches/simpar/worlds/single_box.world
Modified: code/gazebo/branches/simpar/plugins/box_push.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/box_push.cc 2010-06-11 14:22:25 UTC
(rev 8760)
+++ code/gazebo/branches/simpar/plugins/box_push.cc 2010-06-11 18:12:50 UTC
(rev 8761)
@@ -17,12 +17,12 @@
//this->stepTimes.push_back(i);
this->stepTimes.push_back(0.001);
- for (unsigned int i=10; i<=100; i+=10)
- this->stepIters.push_back(i);
+ //for (unsigned int i=10; i<=100; i+=10)
+ this->stepIters.push_back(100);
+ this->stepTypes.push_back("quick");
+ this->stepTypes.push_back("world");
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();
@@ -45,20 +45,6 @@
World::Instance()->DisconnectWorldUpdateStartSignal(
boost::bind(&BoxPush::UpdateCB, this));
-
- /*for (unsigned int i=0; i < this->box->GetChildCount(); i++)
- {
- Body *body = dynamic_cast<Body*>(this->box->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->box->Reset();
- */
}
public: void Load()
@@ -67,6 +53,7 @@
this->physics = World::Instance()->GetPhysicsEngine();
this->count = 0;
this->velReached = false;
+ this->skipCount = 10;
World::Instance()->ConnectWorldUpdateStartSignal(
boost::bind(&BoxPush::UpdateCB, this));
@@ -83,6 +70,10 @@
World::Instance()->ConnectWorldUpdateStartSignal(
boost::bind(&BoxPush::UpdateCB, this));
+ this->physics->SetStepType( *this->stepTypesIter );
+ this->physics->SetStepTime( *this->stepTimesIter );
+ this->physics->SetSORPGSIters( *this->stepItersIter );
+
std::cout << "Type[" << *this->stepTypesIter << "] "
<< "Time[" << *this->stepTimesIter << "] "
<< "Iters[" << *this->stepItersIter << "]\n";
@@ -91,6 +82,12 @@
public: void UpdateCB()
{
+ if (this->skipCount > 0)
+ {
+ this->skipCount--;
+ return;
+ }
+
Time simTime = Simulator::Instance()->GetSimTime();
Pose3d pose = this->box->GetWorldPose();
Body *body = (Body*)this->box->GetChild(0);
@@ -148,6 +145,8 @@
this->physics->SetStepTime( *this->stepTimesIter );
this->physics->SetSORPGSIters( *this->stepItersIter );
+ this->skipCount = 10;
+
this->count++;
std::cout << "Type[" << *this->stepTypesIter << "] "
<< "Time[" << *this->stepTimesIter << "] "
@@ -178,6 +177,7 @@
private: PhysicsEngine *physics;
private: unsigned int count;
private: bool velReached;
+ private: int skipCount;
};
GZ_REGISTER_HANDLER("BoxPush", BoxPush)
Modified: code/gazebo/branches/simpar/server/Quatern.cc
===================================================================
--- code/gazebo/branches/simpar/server/Quatern.cc 2010-06-11 14:22:25 UTC
(rev 8760)
+++ code/gazebo/branches/simpar/server/Quatern.cc 2010-06-11 18:12:50 UTC
(rev 8761)
@@ -414,6 +414,9 @@
this->z = 0;
if (!finite(this->u))
this->u = 1;
+
+ if (this->u == 0 && this->x == 0 && this->y == 0 && this->z == 0)
+ this->u = 1;
}
Modified: code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc 2010-06-11
14:22:25 UTC (rev 8760)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc 2010-06-11
18:12:50 UTC (rev 8761)
@@ -114,6 +114,9 @@
pp.Correct();
+ if ( isnan(pp.pos.x) || isnan(pp.pos.y) || isnan(pp.pos.z) )
+ printf("NAN!!!\n");
+
self->SetWorldPose(pp, false);
}
Modified: code/gazebo/branches/simpar/worlds/3stacks.world
===================================================================
--- code/gazebo/branches/simpar/worlds/3stacks.world 2010-06-11 14:22:25 UTC
(rev 8760)
+++ code/gazebo/branches/simpar/worlds/3stacks.world 2010-06-11 18:12:50 UTC
(rev 8761)
@@ -15,14 +15,14 @@
<verbosity>5</verbosity>
<physics:ode>
- <stepTime>0.01</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.00000000001</cfm>
<erp>0.1</erp>
- <quickStep>true</quickStep>
- <quickStepIters>10</quickStepIters>
- <quickStepW>1.3</quickStepW>
- <contactSurfaceLayer>0.001</contactSurfaceLayer>
+ <stepType>robust</stepType>
+ <stepIters>10</stepIters>
+ <stepW>1.3</stepW>
+ <contactSurfaceLayer>0.00</contactSurfaceLayer>
</physics:ode>
<rendering:gui>
@@ -44,15 +44,13 @@
<shadows>false</shadows>
</rendering:ogre>
-
- <!--
<model:physical name="board">
<xyz>1 1.5 99.05</xyz>
<static>false</static>
<body:box name="body">
<geom:box name="geom">
<size>0.5 3.0 0.1</size>
- <mass>1.0</mass>
+ <mass>100.0</mass>
<mu1>10.0</mu1>
<mu2>10.0</mu2>
<kp>100000000.0</kp>
@@ -66,10 +64,7 @@
</geom:box>
</body:box>
</model:physical>
- -->
-
-
<model:physical name="box1_model">
<xyz>1 1.5 0.5</xyz>
<canonicalBody>box1_body</canonicalBody>
Modified: code/gazebo/branches/simpar/worlds/single_box.world
===================================================================
--- code/gazebo/branches/simpar/worlds/single_box.world 2010-06-11 14:22:25 UTC
(rev 8760)
+++ code/gazebo/branches/simpar/worlds/single_box.world 2010-06-11 18:12:50 UTC
(rev 8761)
@@ -20,8 +20,8 @@
<cfm>10e-10</cfm>
<erp>0.2</erp>
- <stepType>quick</stepType>
- <stepIters>10</stepIters>
+ <stepType>robust</stepType>
+ <stepIters>100</stepIters>
<stepW>1.3</stepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.0</contactSurfaceLayer>
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
