Revision: 8742
http://playerstage.svn.sourceforge.net/playerstage/?rev=8742&view=rev
Author: natepak
Date: 2010-06-01 06:22:38 +0000 (Tue, 01 Jun 2010)
Log Message:
-----------
Updates
Modified Paths:
--------------
code/gazebo/branches/simpar/plugins/pioneer_circle.cc
code/gazebo/branches/simpar/plugins/pioneer_spiral.cc
code/gazebo/branches/simpar/plugins/pioneer_spiral.hh
code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc
code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
code/gazebo/branches/simpar/worlds/models/pioneer2dx.model
code/gazebo/branches/simpar/worlds/pioneer2dx.world
Modified: code/gazebo/branches/simpar/plugins/pioneer_circle.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_circle.cc 2010-05-31
21:18:10 UTC (rev 8741)
+++ code/gazebo/branches/simpar/plugins/pioneer_circle.cc 2010-06-01
06:22:38 UTC (rev 8742)
@@ -14,9 +14,6 @@
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));
@@ -44,9 +41,6 @@
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";
@@ -54,13 +48,13 @@
public: void UpdateCB()
{
- if (this->leftWheel->GetAngularVel().y < 0.2)
+ if (this->leftWheel->GetAngularVel().z < 4.2)
{
- this->leftWheel->SetTorque(Vector3(0.0, 0.0, 0.08));
+ this->leftWheel->SetTorque(Vector3(0.0, 0.0, 0.2));
}
- if (this->rightWheel->GetAngularVel().y > 0.1)
- this->rightWheel->SetTorque(Vector3(0.0, 0.0, -0.08));
+ if (this->rightWheel->GetAngularVel().z > 4.0)
+ this->rightWheel->SetTorque(Vector3(0.0, 0.0, -0.2));
//this->rightWheel->SetTorque(Vector3(0.0, 0.0, 0.01));
//this->rightWheel->SetTorque(Vector3(0.0, 0.0, 0));
@@ -68,12 +62,6 @@
//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;
Modified: code/gazebo/branches/simpar/plugins/pioneer_spiral.cc
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_spiral.cc 2010-05-31
21:18:10 UTC (rev 8741)
+++ code/gazebo/branches/simpar/plugins/pioneer_spiral.cc 2010-06-01
06:22:38 UTC (rev 8742)
@@ -30,7 +30,7 @@
void PioneerSpiral::Load()
{
- std::string model_name = "pioneer2dx_model";
+ std::string model_name = "pioneer";
this->robot = (Model*)World::Instance()->GetEntityByName(model_name);
if (this->robot)
@@ -42,16 +42,19 @@
Geom *leftWheelGeom = this->leftWheel->GetGeom("left_wheel_geom");
Geom *rightWheelGeom = this->rightWheel->GetGeom("right_wheel_geom");
- leftWheelGeom->SetContactsEnabled(true);
+ /*leftWheelGeom->SetContactsEnabled(true);
rightWheelGeom->SetContactsEnabled(true);
leftWheelGeom->ConnectContactCallback(
boost::bind(&PioneerSpiral::LeftContactCB, this, _1));
rightWheelGeom->ConnectContactCallback(
boost::bind(&PioneerSpiral::RightContactCB, this, _1));
+ */
World::Instance()->ConnectWorldUpdateStartSignal(
boost::bind(&PioneerSpiral::UpdateCB, this));
+
+ this->prevTime = Simulator::Instance()->GetRealTime();
}
else
std::cerr << "Unable to find model[" << model_name << "]\n";
@@ -59,36 +62,35 @@
void PioneerSpiral::UpdateCB()
{
- //std::cout << "torqu[" << this->leftWheel->GetAngularVel() << "]\n";
- if (this->leftWheel->GetAngularVel().GetLength() < 0.1)
+ Time currTime = Simulator::Instance()->GetRealTime();
+ if (currTime - prevTime < Time(0,200000000))
{
- printf("HERE\n");
- this->leftWheel->SetTorque(Vector3(0.0,1.0,0));
- this->rightWheel->SetTorque(Vector3(0.0,0.5,0));
+ this->leftWheel->SetTorque(Vector3(0.0,0.0,.01));
+ this->rightWheel->SetTorque(Vector3(0.0,0.0,.01));
}
+
}
-void PioneerSpiral::LeftContactCB(const Contact &contact)
+/*void PioneerSpiral::LeftContactCB(const Contact &contact)
{
- /*std::cout << "Left contact\n";
+ std::cout << "Left contact\n";
for (unsigned int i=0; i < contact.positions.size(); i++)
{
std::cout << " Pos[" << contact.positions[i] << "] "
<< "Norm[" << contact.normals[i] << "] "
<< "Depth[" << contact.depths[i] << "]\n";
}
- */
}
void PioneerSpiral::RightContactCB(const Contact &contact)
{
- /*std::cout << "Right contact\n";
+ std::cout << "Right contact\n";
for (unsigned int i=0; i < contact.positions.size(); i++)
{
std::cout << " Pos[" << contact.positions[i] << "] "
<< "Norm[" << contact.normals[i] << "] "
<< "Depth[" << contact.depths[i] << "]\n";
}
- */
}
+*/
Modified: code/gazebo/branches/simpar/plugins/pioneer_spiral.hh
===================================================================
--- code/gazebo/branches/simpar/plugins/pioneer_spiral.hh 2010-05-31
21:18:10 UTC (rev 8741)
+++ code/gazebo/branches/simpar/plugins/pioneer_spiral.hh 2010-06-01
06:22:38 UTC (rev 8742)
@@ -22,6 +22,7 @@
private: Model *robot;
private: Body *leftWheel;
private: Body *rightWheel;
+ private: Time prevTime;
};
}
Modified: code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc 2010-05-31
21:18:10 UTC (rev 8741)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEBody.cc 2010-06-01
06:22:38 UTC (rev 8742)
@@ -407,14 +407,16 @@
if (this->bodyId)
{
const dReal *dvel;
+ dReal result[3];
this->physicsEngine->LockMutex();
dvel = dBodyGetLinearVel(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];
+ vel.x = result[0];
+ vel.y = result[1];
+ vel.z = result[2];
}
return vel;
@@ -449,16 +451,9 @@
this->physicsEngine->UnlockMutex();
dBodyVectorFromWorld(this->bodyId, dvel[0], dvel[1], dvel[2], result);
- 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);
+ vel.x = result[0];
+ vel.y = result[1];
+ vel.z = result[2];
}
return vel;
Modified: code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
2010-05-31 21:18:10 UTC (rev 8741)
+++ code/gazebo/branches/simpar/server/physics/ode/ODEPhysics.cc
2010-06-01 06:22:38 UTC (rev 8742)
@@ -725,10 +725,10 @@
continue;
contact.geom = self->contactGeoms[i];
- //contact.surface.mode = dContactSlip1 | dContactSlip2 |
- // dContactSoftERP | dContactSoftCFM |
- // dContactBounce | dContactMu2 |
dContactApprox1;
- contact.surface.mode = dContactSoftERP | dContactSoftCFM |
dContactApprox1;
+ contact.surface.mode = dContactSlip1 | dContactSlip2 |
+ dContactSoftERP | dContactSoftCFM |
+ dContactBounce | dContactMu2 | dContactApprox1;
+ //contact.surface.mode = dContactSoftERP | dContactSoftCFM |
dContactApprox1 | dContactSlip1 | dContactSlip2;
// with dContactSoftERP | dContactSoftCFM the test_pr2_collision
overshoots the cup
// Compute the CFM and ERP by assuming the two bodies form a
Modified: code/gazebo/branches/simpar/worlds/models/pioneer2dx.model
===================================================================
--- code/gazebo/branches/simpar/worlds/models/pioneer2dx.model 2010-05-31
21:18:10 UTC (rev 8741)
+++ code/gazebo/branches/simpar/worlds/models/pioneer2dx.model 2010-06-01
06:22:38 UTC (rev 8742)
@@ -67,7 +67,8 @@
<geom:cylinder name="left_wheel_geom">
<size>0.075 0.05</size>
<mass>0.5</mass>
- <mu1>0.5</mu1>
+ <mu1>1.0</mu1>
+ <mu2>1.0</mu2>
<visual>
<rpy>-90 0 0</rpy>
@@ -102,7 +103,8 @@
<geom:cylinder name="right_wheel_geom">
<size>0.075 0.05</size>
<mass>0.5</mass>
- <mu1>0.5</mu1>
+ <mu1>0.4</mu1>
+ <mu2>0.4</mu2>
<visual>
<rpy>-90 0 0</rpy>
@@ -135,7 +137,7 @@
<geom:sphere name="castor_geom">
<size>0.04</size>
<mass>0.5</mass>
- <mu1>0.5</mu1>
+ <mu1>1000000.5</mu1>
<visual>
<scale>0.04 0.04 0.04</scale>
Modified: code/gazebo/branches/simpar/worlds/pioneer2dx.world
===================================================================
--- code/gazebo/branches/simpar/worlds/pioneer2dx.world 2010-05-31 21:18:10 UTC
(rev 8741)
+++ code/gazebo/branches/simpar/worlds/pioneer2dx.world 2010-06-01 06:22:38 UTC
(rev 8742)
@@ -24,7 +24,7 @@
<cfm>0.0000000001</cfm>
<erp>0.2</erp>
- <stepType>quick</stepType>
+ <stepType>world</stepType>
<stepIters>100</stepIters>
<stepW>1.3</stepW>
<contactMaxCorrectingVel>100.0</contactMaxCorrectingVel>
@@ -51,6 +51,11 @@
<body:plane name="plane1_body">
<geom:plane name="plane1_geom">
<normal>0 0 1</normal>
+ <mu1>50.0</mu1>
+ <mu2>50.0</mu2>
+ <kp>1000000000.0</kp>
+ <kd>1.0</kd>
+
<size>2000 2000</size>
<segments>10 10</segments>
<uvTile>2000 2000</uvTile>
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
