Revision: 8758
http://playerstage.svn.sourceforge.net/playerstage/?rev=8758&view=rev
Author: natepak
Date: 2010-06-10 00:29:44 +0000 (Thu, 10 Jun 2010)
Log Message:
-----------
Updates
Modified Paths:
--------------
code/gazebo/branches/simpar/plugins/pioneer_gripper.cc
code/gazebo/branches/simpar/worlds/pioneer2dx_gripper.world
Modified: code/gazebo/branches/simpar/plugins/pioneer_gripper.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_gripper.cc 2010-06-09
15:50:46 UTC (rev 8757)
+++ code/gazebo/branches/simpar/plugins/pioneer_gripper.cc 2010-06-10
00:29:44 UTC (rev 8758)
@@ -79,15 +79,19 @@
this->physics = World::Instance()->GetPhysicsEngine();
if (LOG)
- Logger::Instance()->AddLog("pioneer","/tmp/pioneer.log");
+ {
+
Logger::Instance()->AddLog("pioneer_gripper::left_paddle_body","/tmp/pioneer_left.log");
+
Logger::Instance()->AddLog("pioneer_gripper::right_paddle_body","/tmp/pioneer_right.log");
+ Logger::Instance()->AddLog("custom_box","/tmp/box.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";
@@ -96,59 +100,33 @@
this->rightForce = 0;
}
- public: void SpawnBox(double x, double y, double z=1)
+ public: void Open()
{
- std::ostringstream model;
+ Vector3 leftVel = this->leftPaddle->GetWorldLinearVel();
+ Vector3 rightVel = this->rightPaddle->GetWorldLinearVel();
- 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;
+ this->leftForce = (0.08 - leftVel.y) * 0.5;
+ this->rightForce = (-0.08 - rightVel.y) * 0.5;
}
- public: void Open()
+ public: void Close()
{
- std::cout << "Open ";
- this->leftForce = 0.01;
- this->rightForce = -0.01;
+ Vector3 leftVel = this->leftPaddle->GetWorldLinearVel();
+ Vector3 rightVel = this->rightPaddle->GetWorldLinearVel();
+
+ this->leftForce = (-0.08 - leftVel.y) * 0.5;
+ this->rightForce = (0.08 - rightVel.y) * 0.5;
}
- public: void Close()
+ public: void Grip()
{
- std::cout << "Close ";
- this->leftForce = -1.00;
- this->rightForce = 1.00;
+ this->leftForce = -10.0;
+ this->rightForce = 10.0;
}
public: void Lift()
{
- std::cout << "Lift ";
- this->liftBody->SetForce(Vector3(0,0,5.0));
+ this->liftBody->SetForce(Vector3(0,0,10.0));
}
public: bool IsOpen()
@@ -161,11 +139,8 @@
{
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()
@@ -175,19 +150,22 @@
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);
+ this->Close();
- if (World::Instance()->GetEntityByName("custom_box"))
+ if (this->IsClosed())
{
- this->Close();
- if (this->IsClosed())
- this->Lift();
+ this->Grip();
+ this->Lift();
}
- else
- this->Open();
- std::cout << "\n";
+ if (simTime - this->prevTime > Time(60,0))
+ {
+ Logger::Instance()->RemoveLog("pioneer_gripper::left_paddle_body");
+ Logger::Instance()->RemoveLog("pioneer_gripper::right_paddle_body");
+ Logger::Instance()->RemoveLog("custom_box");
+ printf("Done\n");
+ delete this;
+ }
}
Modified: code/gazebo/branches/simpar/worlds/pioneer2dx_gripper.world
===================================================================
--- code/gazebo/branches/simpar/worlds/pioneer2dx_gripper.world 2010-06-09
15:50:46 UTC (rev 8757)
+++ code/gazebo/branches/simpar/worlds/pioneer2dx_gripper.world 2010-06-10
00:29:44 UTC (rev 8758)
@@ -19,15 +19,15 @@
<verbosity>5</verbosity>
<physics:ode>
- <stepTime>0.0001</stepTime>
+ <stepTime>0.001</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
- <stepType>world</stepType>
- <stepIters>100</stepIters>
- <stepW>1.3</stepW>
- <contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
+ <stepType>robust</stepType>
+ <stepIters>20</stepIters>
+ <stepW>.3</stepW>
+ <contactMaxCorrectingVel>1.0</contactMaxCorrectingVel>
<contactSurfaceLayer>0.00</contactSurfaceLayer>
</physics:ode>
@@ -59,12 +59,35 @@
</body:plane>
</model:physical>
+ <model:physical name='custom_box'>
+ <xyz>0.15 0 0.1</xyz>
+ <rpy>0 0 0</rpy>
+ <body:box name='body'>
+ <geom:box name='geom'>
+ <size>0.1 0.10 0.2</size>
+ <mass>0.01</mass>
+ <kp>100000000.0</kp>
+ <kd>1.0</kd>
+ <bounce>0</bounce>
+ <bounceVel>100000</bounceVel>
+ <slip1>0.01</slip1>
+ <slip2>0.01</slip2>
+ <visual>
+ <size>0.1 0.1 0.2</size>
+ <mesh>unit_box</mesh>
+ <material>Gazebo/Rocky</material>
+ </visual>
+ </geom:box>
+ </body:box>
+ </model:physical>
+
+
<model:physical name="pioneer_gripper">
- <xyz>0 0 0.065</xyz>
+ <xyz>0 0 0.0</xyz>
<rpy>0.0 0.0 0.0</rpy>
<body:box name="gripper_base_body">
- <xyz>0.0 0 0.0</xyz>
+ <xyz>0.0 0 0.115</xyz>
<geom:box name="gripper_base_geom">
<xyz>0 0 0</xyz>
<size>0.053 0.123 0.130</size>
@@ -78,7 +101,7 @@
</body:box>
<body:box name="lift_body">
- <xyz>0.061 0 0</xyz>
+ <xyz>0.061 0 0.0315</xyz>
<geom:box name="lift_geom">
<xyz>0 0 0</xyz>
<size>0.069 0.313 0.063</size>
@@ -92,7 +115,7 @@
</body:box>
<body:box name="left_paddle_body">
- <xyz>0.145 0.0135 0</xyz>
+ <xyz>0.145 0.135 0.0315</xyz>
<geom:box name="left_paddle_geom">
<xyz>0 0 0</xyz>
<size>0.095 0.025 0.039</size>
@@ -106,7 +129,7 @@
</body:box>
<body:box name="right_paddle_body">
- <xyz>0.145 -0.0135 0</xyz>
+ <xyz>0.145 -0.135 0.0315</xyz>
<geom:box name="right_paddle_geom">
<xyz>0 0 0</xyz>
<size>0.095 0.025 0.039</size>
@@ -120,27 +143,25 @@
</geom:box>
</body:box>
- <!--<joint:hinge name="fixed">
+ <joint:hinge name="fixed">
<body1>gripper_base_body</body1>
<body2>world</body2>
<anchor>gripper_base_body</anchor>
- <anchorOffset>0 0 -0.130</anchorOffset>
+ <anchorOffset>0 0 0.0</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>lift_body</body2>
<anchor>gripper_base_body</anchor>
<axis>0 0 1</axis>
- <lowStop>-0.1</lowStop>
- <highStop>0.1</highStop>
+ <lowStop>-0.05</lowStop>
+ <highStop>0.05</highStop>
<erp>0.3</erp>
<cfm>10e-5</cfm>
</joint:slider>
@@ -151,8 +172,8 @@
<anchor>lift_body</anchor>
<anchorOffset>0.085 0 0 </anchorOffset>
<axis>0 1 0</axis>
- <lowStop>-0.12</lowStop>
- <highStop>0.0</highStop>
+ <lowStop>0.0</lowStop>
+ <highStop>0.12</highStop>
<erp>0.1</erp>
<cfm>10e-5</cfm>
</joint:slider>
@@ -163,8 +184,8 @@
<anchor>lift_body</anchor>
<anchorOffset>0.085 0 0 </anchorOffset>
<axis>0 1 0</axis>
- <lowStop>0.0</lowStop>
- <highStop>0.12</highStop>
+ <lowStop>-0.12</lowStop>
+ <highStop>0.0</highStop>
<erp>0.1</erp>
<cfm>10e-5</cfm>
</joint:slider>
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
