Commits

John Hsu committed 15b4ae1 Merge

merging from gazebo_1.5

  • Participants
  • Parent commits ac11bb2, 1af6c7f
  • Branches urdf_material

Comments (0)

Files changed (5)

File gazebo/physics/Joint_TEST.cc

 };
 
 ////////////////////////////////////////////////////////////////////////
-// Test world template
+// Create a joint between link and world
+// Apply force and check acceleration for correctness
 ////////////////////////////////////////////////////////////////////////
+TEST_F(Joint_TEST, JointTorqueTest)
+{
+  // Load our inertial test world
+  Load("worlds/joint_test.world", true);
+
+  // Get a pointer to the world, make sure world loads
+  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(), "ode");
+
+  // create some fake links
+  physics::ModelPtr model = world->GetModel("model_1");
+  ASSERT_TRUE(model != NULL);
+  physics::LinkPtr link = model->GetLink("link_1");
+  ASSERT_TRUE(link != NULL);
+
+  physics::LinkPtr parentLink;
+  physics::LinkPtr childLink(link);
+  physics::JointPtr joint;
+  math::Pose anchor;
+  double upper = M_PI;
+  double lower = -M_PI;
+
+  {
+    // create a joint
+    {
+      joint = world->GetPhysicsEngine()->CreateJoint(
+        "revolute", model);
+      joint->Attach(parentLink, childLink);
+      // load adds the joint to a vector of shared pointers kept
+      // in parent and child links, preventing joint from being destroyed.
+      joint->Load(parentLink, childLink, anchor);
+      // joint->SetAnchor(0, anchor);
+      joint->SetAxis(0, math::Vector3(1, 0, 0));
+      joint->SetHighStop(0, upper);
+      joint->SetLowStop(0, lower);
+
+      if (parentLink)
+        joint->SetName(parentLink->GetName() + std::string("_") +
+                       childLink->GetName() + std::string("_joint"));
+      else
+        joint->SetName(std::string("world_") +
+                       childLink->GetName() + std::string("_joint"));
+      joint->Init();
+    }
+
+    double lastV = 0;
+    double dt = world->GetPhysicsEngine()->GetStepTime();
+    for (unsigned int i = 0; i < 10; ++i)
+    {
+      double torque = 1.3;
+      joint->SetForce(0, torque);
+      world->StepWorld(1);
+      double curV = joint->GetVelocity(0);
+      double accel = (curV - lastV) / dt;
+      gzdbg << i << " : " << curV << " : " << (curV - lastV) / dt << "\n";
+      lastV = curV;
+      EXPECT_NEAR(accel, torque / link->GetInertial()->GetIXX(), TOL);
+    }
+
+    // remove the joint
+    {
+      bool paused = world->IsPaused();
+      world->SetPaused(true);
+      if (joint)
+      {
+        // reenable collision between the link pair
+        physics::LinkPtr parent = joint->GetParent();
+        physics::LinkPtr child = joint->GetChild();
+        if (parent)
+          parent->SetCollideMode("all");
+        if (child)
+          child->SetCollideMode("all");
+
+        joint->Detach();
+        joint.reset();
+      }
+      world->SetPaused(paused);
+    }
+  }
+
+  {
+    // create a joint
+    {
+      joint = world->GetPhysicsEngine()->CreateJoint(
+        "revolute", model);
+      joint->Attach(parentLink, childLink);
+      // load adds the joint to a vector of shared pointers kept
+      // in parent and child links, preventing joint from being destroyed.
+      joint->Load(parentLink, childLink, anchor);
+      // joint->SetAnchor(0, anchor);
+      joint->SetAxis(0, math::Vector3(0, 0, 1));
+      joint->SetHighStop(0, upper);
+      joint->SetLowStop(0, lower);
+
+      if (parentLink)
+        joint->SetName(parentLink->GetName() + std::string("_") +
+                       childLink->GetName() + std::string("_joint"));
+      else
+        joint->SetName(std::string("world_") +
+                       childLink->GetName() + std::string("_joint"));
+      joint->Init();
+    }
+
+    double lastV = 0;
+    double dt = world->GetPhysicsEngine()->GetStepTime();
+    for (unsigned int i = 0; i < 10; ++i)
+    {
+      double torque = 1.3;
+      joint->SetForce(0, torque);
+      world->StepWorld(1);
+      double curV = joint->GetVelocity(0);
+      double accel = (curV - lastV) / dt;
+      gzdbg << i << " : " << curV << " : " << (curV - lastV) / dt << "\n";
+      lastV = curV;
+      EXPECT_NEAR(accel, torque / link->GetInertial()->GetIZZ(), TOL);
+    }
+
+    // remove the joint
+    {
+      bool paused = world->IsPaused();
+      world->SetPaused(true);
+      if (joint)
+      {
+        // reenable collision between the link pair
+        physics::LinkPtr parent = joint->GetParent();
+        physics::LinkPtr child = joint->GetChild();
+        if (parent)
+          parent->SetCollideMode("all");
+        if (child)
+          child->SetCollideMode("all");
+
+        joint->Detach();
+        joint.reset();
+      }
+      world->SetPaused(paused);
+    }
+  }
+}
+
 TEST_F(Joint_TEST, JointCreationDestructionTest)
 {
   // Load our inertial test world
   double upper = M_PI;
   double lower = -M_PI;
 
-  double residentLast, shareLast;
-  double residentCur, shareCur;
+  double residentLast = 0, shareLast = 0;
+  double residentCur = 0, shareCur = 0;
 
   for (unsigned int i = 0; i < 100; ++i)
   {
         joint->SetName(std::string("world_") +
                        childLink->GetName() + std::string("_joint"));
       joint->Init();
+      joint->SetAxis(0, axis);
     }
     // remove the joint
     {
     this->GetMemInfo(residentCur, shareCur);
     if (i > 1)  // give it 2 cycles to stabilize
     {
-      EXPECT_EQ(residentCur, residentLast);
-      EXPECT_EQ(shareCur, shareLast);
+      EXPECT_TRUE(math::equal(residentCur, residentLast));
+      EXPECT_TRUE(math::equal(shareCur, shareLast));
     }
     // gzdbg << "memory res[" << residentCur
     //       << "] shr[" << shareCur

File gazebo/physics/ode/ODEHingeJoint.cc

 }
 
 //////////////////////////////////////////////////
-void ODEHingeJoint::SetAxis(int /*index*/, const math::Vector3 &_axis)
+void ODEHingeJoint::SetAxis(int _index, const math::Vector3 &_axis)
 {
+  ODEJoint::SetAxis(_index, _axis);
+
   if (this->childLink)
     this->childLink->SetEnabled(true);
   if (this->parentLink)

File gazebo/physics/ode/ODEJoint.cc

 }
 
 //////////////////////////////////////////////////
+void ODEJoint::SetAxis(int _index, const math::Vector3 &_axis)
+{
+  if (_index == 0)
+    this->sdf->GetElement("axis")->GetElement("xyz")->Set(_axis);
+  else if (_index == 1)
+    this->sdf->GetElement("axis2")->GetElement("xyz")->Set(_axis);
+  else
+    gzerr << "SetAxis index [" << _index << "] out of bounds\n";
+}
+
+//////////////////////////////////////////////////
 void ODEJoint::SetAttribute(Attribute _attr, int _index, double _value)
 {
   switch (_attr)

File gazebo/physics/ode/ODEJoint.hh

                                         double _value);
 
       // Documentation inherited.
+      public: virtual void SetAxis(int _index, const math::Vector3 &_axis);
+
+      // Documentation inherited.
       public: virtual void SetAttribute(const std::string &_key, int _index,
                                         const boost::any &_value);
 

File test/worlds/joint_test.world

+<?xml version="1.0" ?>
+<sdf version="1.3">
+    <world name="default">
+        <physics type="ode">
+            <gravity>0 0 0</gravity>
+            <update_rate>1000</update_rate>
+            <ode>
+                <solver>
+                    <type>quick</type>
+                    <dt>0.001</dt>
+                    <iters>1000</iters>
+                    <sor>1.0</sor>
+                </solver>
+                <constraints>
+                    <cfm>0.0</cfm>
+                    <erp>0.2</erp>
+                    <contact_max_correcting_vel>100.0</contact_max_correcting_vel>
+                    <contact_surface_layer>0.0</contact_surface_layer>
+                </constraints>
+            </ode>
+        </physics>
+        <!-- A global light source -->
+        <include>
+            <uri>model://sun</uri>
+        </include>
+        <model name="model_1">
+            <pose>0 0 0 0 0 0</pose>
+            <link name="link_1">
+                <pose>0 0 0 0 0 0</pose>
+                <inertial>
+                    <pose>0 0 0 0 0 0</pose>
+                    <inertia>
+                        <ixx>200.000000</ixx>
+                        <ixy>0.000000</ixy>
+                        <ixz>0.000000</ixz>
+                        <iyy>300.000000</iyy>
+                        <iyz>0.000000</iyz>
+                        <izz>400.000000</izz>
+                    </inertia>
+                    <mass>1.000000</mass>
+                </inertial>
+                <visual name="visual_cylinder">
+                    <pose>0 0 -0.5 0 0 0</pose>
+                    <geometry>
+                        <cylinder>
+                            <radius>0.100000</radius>
+                            <length>1.000000</length>
+                        </cylinder>
+                    </geometry>
+                    <material>
+                        <script>Gazebo/Green</script>
+                    </material>
+                </visual>
+                <collision name="collision_cylinder">
+                    <pose>0 0 -0.5 0 0 0</pose>
+                    <geometry>
+                        <cylinder>
+                            <radius>0.100000</radius>
+                            <length>1.000000</length>
+                        </cylinder>
+                    </geometry>
+                </collision>
+            </link>
+            <static>0</static>
+        </model>
+    </world>
+</sdf>