Commits

Steven Peters committed d3ff735

Started implementing a test for evaluating numerical stability of the physics engine. Partially implemented BulletLink::AddForce, but it doesn't yet account for frame rotation of the link.

  • Participants
  • Parent commits 20efe85
  • Branches physics_stability

Comments (0)

Files changed (3)

gazebo/physics/PhysicsEngine_TEST.cc

 {
   public: void OnPhysicsMsgResponse(ConstResponsePtr &_msg);
   public: void PhysicsEngineParam(const std::string &_physicsEngine);
+  public: void PhysicsEngineStability(const std::string &_physics);
   public: static msgs::Physics physicsPubMsg;
   public: static msgs::Physics physicsResponseMsg;
 };
 }
 #endif  // HAVE_BULLET
 
+/////////////////////////////////////////////////
+// PhysicsEngineStability: simulate harmonic oscillators at various time steps
+// and verify stability of system energy.
+void PhysicsEngineTest::PhysicsEngineStability(const std::string &_physics)
+{
+  Load("worlds/blank.world", true, _physics);
+  physics::WorldPtr world = physics::get_world("default");
+  ASSERT_TRUE(world != NULL);
+
+  // Verify physics engine type
+  physics::PhysicsEnginePtr physics = world->GetPhysicsEngine();
+  ASSERT_TRUE(physics != NULL);
+  EXPECT_EQ(physics->GetType(), _physics);
+
+  // Turn off gravity
+  physics->SetGravity(math::Vector3::Zero);
+
+  // Spawn a box up in the air
+  math::Pose pose0(2, 2, 2, 0, 0, 0);
+  std::string name = "test_box";
+  SpawnBox(name, math::Vector3(1, 1, 1), pose0.pos, pose0.rot.GetAsEuler());
+  physics::ModelPtr model = world->GetModel(name);
+  ASSERT_TRUE(model != NULL);
+  physics::LinkPtr link = model->GetLink();
+  ASSERT_TRUE(link != NULL);
+  double mass = link->GetInertial()->GetMass();
+
+  // Make a PID controller for translational position
+  double fMax = 1e10;
+  math::Vector3 offset(1, 1, 1);
+  offset *= 0.001;
+  math::Pose poseDes(pose0);
+  poseDes.pos = offset + poseDes.pos;
+  math::Vector3 controllerHz(300, 310, 320);
+  math::Vector3 gainP;
+  gainP = controllerHz * 2.0 * M_PI;
+  gainP = gainP*gainP;
+  gzerr << "gainP " << gainP << '\n';
+  // math::Vector3 gainP(100.0, 1000.0, 10000.0);
+  common::PID pidX = common::PID(gainP.x, 0.0, 0.0, 0.0, 0.0, fMax, -fMax);
+  common::PID pidY = common::PID(gainP.y, 0.0, 0.0, 0.0, 0.0, fMax, -fMax);
+  common::PID pidZ = common::PID(gainP.z, 0.0, 0.0, 0.0, 0.0, fMax, -fMax);
+  pidX.SetCmd(poseDes.pos.x);
+  pidY.SetCmd(poseDes.pos.y);
+  pidZ.SetCmd(poseDes.pos.z);
+
+  math::Vector3 error0 = pose0.pos - poseDes.pos;
+  math::Vector3 energy0 = 1/2 * gainP * error0 * error0;
+
+
+  unsigned int steps = 200;
+  unsigned int i;
+  math::Vector3 error;
+  double timeLast = world->GetSimTime().Double();
+  double timeNow;
+  math::Vector3 force, energy, velocity;
+  for(i = 0; i < steps; ++i)
+  {
+    world->StepWorld(1);
+    timeNow = world->GetSimTime().Double();
+    error = link->GetWorldPose().pos - poseDes.pos;
+    force.x = pidX.Update(error.x, timeNow-timeLast);
+    force.y = pidY.Update(error.y, timeNow-timeLast);
+    force.z = pidZ.Update(error.z, timeNow-timeLast);
+    link->AddForce(force);
+    timeLast = timeNow;
+    velocity = link->GetWorldLinearVel();
+    energy = 0.5 * (gainP * error*error  +  mass * velocity*velocity);
+    gzerr << "energy " << energy
+          << " velocity " << velocity << '\n';
+    // getchar();
+
+  }
+  gzerr << "mass " << mass << '\n';
+}
+
+TEST_F(PhysicsEngineTest, PhysicsEngineStabilityODE)
+{
+  PhysicsEngineStability("ode");
+}
+
+#ifdef HAVE_BULLET
+TEST_F(PhysicsEngineTest, PhysicsEngineStabilityBullet)
+{
+  PhysicsEngineStability("bullet");
+}
+#endif  // HAVE_BULLET
+
 int main(int argc, char **argv)
 {
   ::testing::InitGoogleTest(&argc, argv);

gazebo/physics/bullet/BulletLink.cc

 }*/
 
 /////////////////////////////////////////////////
-void BulletLink::AddForce(const math::Vector3 &/*_force*/)
+void BulletLink::AddForce(const math::Vector3 &_force)
 {
-  gzlog << "BulletLink::AddForce not yet implemented." << std::endl;
+  // Apply force at origin of link frame
+  math::Vector3 cogVec = this->inertial->GetCoG();
+  this->rigidLink->applyForce(BulletTypes::ConvertVector3(_force),
+                              BulletTypes::ConvertVector3(-cogVec));
 }
 
 /////////////////////////////////////////////////

sdf/worlds/empty.world

 <?xml version="1.0" ?>
-<sdf version="1.3">
+<sdf version="1.4">
   <world name="default">
+    <!-- physics parameters -->
+    <physics type="ode">
+      <gravity>0.000000 0.000000 -9.810000</gravity>
+      <max_step_size>0.001</max_step_size>
+      <ode>
+        <solver>
+          <type>quick</type>
+          <iters>1000</iters>
+          <precon_iters>0</precon_iters>
+          <sor>1.300000</sor>
+        </solver>
+        <constraints>
+          <cfm>0.000000</cfm>
+          <erp>1.000000</erp>
+          <contact_max_correcting_vel>0.000000</contact_max_correcting_vel>
+          <contact_surface_layer>0.000000</contact_surface_layer>
+        </constraints>
+      </ode>
+      <bullet>
+        <solver>
+          <iters>1000</iters>
+        </solver>
+      </bullet>
+    </physics>
     <!-- A global light source -->
     <include>
       <uri>model://sun</uri>