[Personalrobots-commit] SF.net SVN: personalrobots:[8219] pkg/trunk

[<hsu...@us...>]

Revision: 8219
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8219&view=rev
Author:   hsujohnhsu
Date:     2008-12-17 00:28:15 +0000 (Wed, 17 Dec 2008)

Log Message:
-----------
 * added patch to use Kd for mimic joints.
 * obtain stable mimic finger tips with light mass, recover time steps of 1ms for tests in gazebo_plugin.

Modified Paths:
--------------
    pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
    pkg/trunk/drivers/simulator/gazebo_plugin/test/test_slide.xml
    pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_camera.world
    pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_scan.world
    pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_slide.world

Modified: pkg/trunk/3rdparty/gazebo/gazebo_patch.diff
===================================================================
--- pkg/trunk/3rdparty/gazebo/gazebo_patch.diff	2008-12-17 00:28:07 UTC (rev 8218)
+++ pkg/trunk/3rdparty/gazebo/gazebo_patch.diff	2008-12-17 00:28:15 UTC (rev 8219)
@@ -899,6 +899,34 @@
  
    return NULL;
  }
+Index: server/physics/Joint.hh
+===================================================================
+--- server/physics/Joint.hh	(revision 7168)
++++ server/physics/Joint.hh	(working copy)
+@@ -163,6 +163,23 @@
+     private: ParamT<bool> *provideFeedbackP;
+     private: ParamT<double> *fudgeFactorP;
+ 
++    /// Added for mimicing other joints
++    private: ParamT<std::string> *mimicJointP;
++    private: ParamT<double> *mimicMultP;
++    private: ParamT<double> *mimicOffsetP;
++    private: ParamT<double> *mimicKpP;
++    private: ParamT<double> *mimicKdP;
++    private: ParamT<double> *mimicFMaxP;
++    private: Joint *mimicJoint;
++    private: bool enableMimic;
++    private: double mimicMult;
++    private: double mimicOffset;
++    private: double mimicKp;
++    private: double mimicKd;
++    private: double mimicFMax;
++    private: double current_time, last_time;
++    private: double current_error, last_error;
++
+     /// Feedback data for this joint
+     private: dJointFeedback *feedback;
+ 
 Index: server/physics/Body.cc
 ===================================================================
 --- server/physics/Body.cc	(revision 7168)
@@ -1159,32 +1187,6 @@
  
  
    // Settle on the new CoM pose
-Index: server/physics/Joint.hh
-===================================================================
---- server/physics/Joint.hh	(revision 7168)
-+++ server/physics/Joint.hh	(working copy)
-@@ -163,6 +163,21 @@
-     private: ParamT<bool> *provideFeedbackP;
-     private: ParamT<double> *fudgeFactorP;
- 
-+    /// Added for mimicing other joints
-+    private: ParamT<std::string> *mimicJointP;
-+    private: ParamT<double> *mimicMultP;
-+    private: ParamT<double> *mimicOffsetP;
-+    private: ParamT<double> *mimicKpP;
-+    private: ParamT<double> *mimicKdP;
-+    private: ParamT<double> *mimicFMaxP;
-+    private: Joint *mimicJoint;
-+    private: bool enableMimic;
-+    private: double mimicMult;
-+    private: double mimicOffset;
-+    private: double mimicKp;
-+    private: double mimicKd;
-+    private: double mimicFMax;
-+
-     /// Feedback data for this joint
-     private: dJointFeedback *feedback;
- 
 Index: server/physics/HingeJoint.cc
 ===================================================================
 --- server/physics/HingeJoint.cc	(revision 7168)
@@ -1510,7 +1512,7 @@
  
    if (**this->provideFeedbackP)
    {
-@@ -147,6 +174,25 @@
+@@ -147,6 +174,29 @@
    {
      this->SetAnchor(anchorVec);
    }
@@ -1529,14 +1531,18 @@
 +    std::cout << this->nameP->GetValue() << " this joint will mimic joint " << this->mimicJointP->GetValue() << std::endl;
 +    if (this->type == Joint::HINGE)
 +    {
-+      ((HingeJoint*)this)->SetParam(dParamFMax, this->mimicFMax);
-+      ((HingeJoint*)this)->SetTorque(0);
++      dynamic_cast<HingeJoint*>(this)->SetParam(dParamFMax, this->mimicFMax);
++      dynamic_cast<HingeJoint*>(this)->SetTorque(0);
 +    }
++    current_time = Simulator::Instance()->GetSimTime();
++    last_time = current_time;
++    current_error = (this->mimicMult*((dynamic_cast<HingeJoint*>(this->mimicJoint))->GetAngle()) - this->mimicOffset - dynamic_cast<HingeJoint*>(this)->GetAngle());
++    last_error = current_error;
 +  }
  }
  
  ////////////////////////////////////////////////////////////////////////////////
-@@ -184,13 +230,39 @@
+@@ -184,13 +234,45 @@
  /// Update the joint
  void Joint::Update()
  {
@@ -1549,10 +1555,16 @@
 +      this->mimicJoint->GetType() == Joint::HINGE &&
 +      this->type                  == Joint::HINGE )
 +  {
-+    cmd = this->mimicKp * (this->mimicMult*(((HingeJoint*)(this->mimicJoint))->GetAngle()) - this->mimicOffset - ((HingeJoint*)this)->GetAngle());
-+    ((HingeJoint*)this)->SetParam(dParamVel, cmd);
++    current_time = Simulator::Instance()->GetSimTime();
++    current_error = (this->mimicMult*((dynamic_cast<HingeJoint*>(this->mimicJoint))->GetAngle()) - this->mimicOffset - dynamic_cast<HingeJoint*>(this)->GetAngle());
+ 
++    cmd =  this->mimicKp * current_error
++         + this->mimicKd * (current_error - last_error) * (current_time - last_time);
++    dynamic_cast<HingeJoint*>(this)->SetParam(dParamVel, cmd);
++    last_error = current_error;
++    last_time = current_time;
 +  }
- 
++
 +  // if (this->type                  == Joint::HINGE )
 +  // {
 +  //   std::cout <<   " joint name " << this->nameP->GetValue()
@@ -1560,9 +1572,9 @@
 +  //             << " mimic offset " << this->mimicOffset
 +  //             << " mimic multip " << this->mimicMult
 +  //             << " mimic     kp " << this->mimicKp
-+  //             <<     "    fmax  " << ((HingeJoint*)this)->GetParam(dParamFMax)
-+  //             <<     "     vel  " << ((HingeJoint*)this)->GetParam(dParamVel)
-+  //             <<     "     ang  " << ((HingeJoint*)this)->GetAngle()
++  //             <<     "    fmax  " << (dynamic_cast<HingeJoint*>this)->GetParam(dParamFMax)
++  //             <<     "     vel  " << (dynamic_cast<HingeJoint*>this)->GetParam(dParamVel)
++  //             <<     "     ang  " << (dynamic_cast<HingeJoint*>this)->GetAngle()
 +  //             << std::endl;
 +  // }
 +
@@ -1579,7 +1591,7 @@
  
    Vector3 start;
    if (this->body1)
-@@ -206,6 +278,7 @@
+@@ -206,6 +288,7 @@
      this->line2->SetPoint(0, start);
      this->line2->Update();
    }
@@ -2861,123 +2873,6 @@
      this->RTTMode="PBuffer";
    }
  }
-Index: server/Model.cc
-===================================================================
---- server/Model.cc	(revision 7168)
-+++ server/Model.cc	(working copy)
-@@ -46,7 +46,12 @@
- #include "ControllerFactory.hh"
- #include "IfaceFactory.hh"
- #include "Model.hh"
-+#include "Simulator.hh"
- 
-+#ifdef TIMING
-+#include "Simulator.hh"// for timing
-+#endif
-+
- using namespace gazebo;
- 
- uint Model::lightNumber = 0;
-@@ -127,7 +132,10 @@
-   if (this->type == "physical")
-     this->LoadPhysical(node);
-   else if (this->type == "renderable")
--    this->LoadRenderable(node);
-+  {
-+    if (Simulator::Instance()->GetRenderEngineEnabled())
-+      this->LoadRenderable(node);
-+  }
-   else if (this->type != "empty")
-   {
-     gzthrow("Invalid model type[" + this->type + "]\n");
-@@ -255,10 +263,11 @@
-   }
-   else
-   {
--    if (!this->lightName.empty())
--    {
--      OgreCreator::SaveLight(p, this->lightName, stream);
--    }
-+    if (Simulator::Instance()->GetRenderEngineEnabled())
-+      if (!this->lightName.empty())
-+      {
-+        OgreCreator::SaveLight(p, this->lightName, stream);
-+      }
-   }
- 
-   if (this->parentBodyNameP && this->myBodyNameP)
-@@ -305,7 +314,7 @@
- 
-   return this->InitChild();
- }
--
-+  
- ////////////////////////////////////////////////////////////////////////////////
- // Update the model
- int Model::Update()
-@@ -316,6 +325,10 @@
- 
-   Pose3d bodyPose, newPose, oldPose;
- 
-+#ifdef TIMING
-+  double tmpT1 = Simulator::Instance()->GetWallTime();
-+#endif
-+
-   for (bodyIter=this->bodies.begin(); bodyIter!=this->bodies.end(); bodyIter++)
-   {
-     if (bodyIter->second)
-@@ -324,6 +337,11 @@
-     }
-   }
- 
-+#ifdef TIMING
-+  double tmpT2 = Simulator::Instance()->GetWallTime();
-+  std::cout << "       bodies dt (" << tmpT2-tmpT1 << ")";
-+#endif
-+
-   for (contIter=this->controllers.begin();
-        contIter!=this->controllers.end(); contIter++)
-   {
-@@ -332,6 +350,11 @@
-       contIter->second->Update();
-   }
- 
-+#ifdef TIMING
-+  double tmpT3 = Simulator::Instance()->GetWallTime();
-+  std::cout << " controllers dt (" << tmpT3-tmpT2 << ")";
-+#endif
-+
-   for (jointIter = this->joints.begin(); jointIter != this->joints.end(); jointIter++)
-   {
-     jointIter->second->Update();
-@@ -350,6 +373,11 @@
-     this->rpyP->SetValue(this->pose.rot);
-   }
- 
-+#ifdef TIMING
-+  double tmpT4 = Simulator::Instance()->GetWallTime();
-+  std::cout << " joints/canonical body dt (" << tmpT4-tmpT3 << ")" << std::endl;
-+#endif
-+
-   return this->UpdateChild();
- }
- 
-@@ -730,10 +758,11 @@
-   body->SetPose(Pose3d());
-   this->bodies[body->GetName()] = body;
- 
--  if ((childNode = node->GetChild("light")))
--  {
--    this->lightName = OgreCreator::CreateLight(childNode, body->GetVisualNode());
--  }
-+  if (Simulator::Instance()->GetRenderEngineEnabled())
-+    if ((childNode = node->GetChild("light")))
-+    {
-+      this->lightName = OgreCreator::CreateLight(childNode, body->GetVisualNode());
-+    }
- 
- }
- 
 Index: server/gui/GLWindow.hh
 ===================================================================
 --- server/gui/GLWindow.hh	(revision 7168)
@@ -3155,6 +3050,123 @@
          case XK_Up:
          case XK_w:
            this->directionVec.x += this->moveAmount;
+Index: server/Model.cc
+===================================================================
+--- server/Model.cc	(revision 7168)
++++ server/Model.cc	(working copy)
+@@ -46,7 +46,12 @@
+ #include "ControllerFactory.hh"
+ #include "IfaceFactory.hh"
+ #include "Model.hh"
++#include "Simulator.hh"
+ 
++#ifdef TIMING
++#include "Simulator.hh"// for timing
++#endif
++
+ using namespace gazebo;
+ 
+ uint Model::lightNumber = 0;
+@@ -127,7 +132,10 @@
+   if (this->type == "physical")
+     this->LoadPhysical(node);
+   else if (this->type == "renderable")
+-    this->LoadRenderable(node);
++  {
++    if (Simulator::Instance()->GetRenderEngineEnabled())
++      this->LoadRenderable(node);
++  }
+   else if (this->type != "empty")
+   {
+     gzthrow("Invalid model type[" + this->type + "]\n");
+@@ -255,10 +263,11 @@
+   }
+   else
+   {
+-    if (!this->lightName.empty())
+-    {
+-      OgreCreator::SaveLight(p, this->lightName, stream);
+-    }
++    if (Simulator::Instance()->GetRenderEngineEnabled())
++      if (!this->lightName.empty())
++      {
++        OgreCreator::SaveLight(p, this->lightName, stream);
++      }
+   }
+ 
+   if (this->parentBodyNameP && this->myBodyNameP)
+@@ -305,7 +314,7 @@
+ 
+   return this->InitChild();
+ }
+-
++  
+ ////////////////////////////////////////////////////////////////////////////////
+ // Update the model
+ int Model::Update()
+@@ -316,6 +325,10 @@
+ 
+   Pose3d bodyPose, newPose, oldPose;
+ 
++#ifdef TIMING
++  double tmpT1 = Simulator::Instance()->GetWallTime();
++#endif
++
+   for (bodyIter=this->bodies.begin(); bodyIter!=this->bodies.end(); bodyIter++)
+   {
+     if (bodyIter->second)
+@@ -324,6 +337,11 @@
+     }
+   }
+ 
++#ifdef TIMING
++  double tmpT2 = Simulator::Instance()->GetWallTime();
++  std::cout << "       bodies dt (" << tmpT2-tmpT1 << ")";
++#endif
++
+   for (contIter=this->controllers.begin();
+        contIter!=this->controllers.end(); contIter++)
+   {
+@@ -332,6 +350,11 @@
+       contIter->second->Update();
+   }
+ 
++#ifdef TIMING
++  double tmpT3 = Simulator::Instance()->GetWallTime();
++  std::cout << " controllers dt (" << tmpT3-tmpT2 << ")";
++#endif
++
+   for (jointIter = this->joints.begin(); jointIter != this->joints.end(); jointIter++)
+   {
+     jointIter->second->Update();
+@@ -350,6 +373,11 @@
+     this->rpyP->SetValue(this->pose.rot);
+   }
+ 
++#ifdef TIMING
++  double tmpT4 = Simulator::Instance()->GetWallTime();
++  std::cout << " joints/canonical body dt (" << tmpT4-tmpT3 << ")" << std::endl;
++#endif
++
+   return this->UpdateChild();
+ }
+ 
+@@ -730,10 +758,11 @@
+   body->SetPose(Pose3d());
+   this->bodies[body->GetName()] = body;
+ 
+-  if ((childNode = node->GetChild("light")))
+-  {
+-    this->lightName = OgreCreator::CreateLight(childNode, body->GetVisualNode());
+-  }
++  if (Simulator::Instance()->GetRenderEngineEnabled())
++    if ((childNode = node->GetChild("light")))
++    {
++      this->lightName = OgreCreator::CreateLight(childNode, body->GetVisualNode());
++    }
+ 
+ }
+ 
 Index: server/World.hh
 ===================================================================
 --- server/World.hh	(revision 7168)
@@ -3240,6 +3252,38 @@
  }
  
  ////////////////////////////////////////////////////////////////////////////////
+Index: server/controllers/factory/Factory.cc
+===================================================================
+--- server/controllers/factory/Factory.cc	(revision 7168)
++++ server/controllers/factory/Factory.cc	(working copy)
+@@ -76,6 +76,8 @@
+ // Initialize the controller
+ void Factory::InitChild()
+ {
++  // initialize newModel to blank
++  strcpy((char*)this->factoryIface->data->newModel,"");
+ }
+ 
+ ////////////////////////////////////////////////////////////////////////////////
+@@ -83,8 +85,10 @@
+ void Factory::UpdateChild()
+ {
+   // If there is a string, then add the contents to the world
++  this->factoryIface->Lock(1);
+   if (strcmp((const char*)this->factoryIface->data->newModel,"")!=0)
+   {
++    //std::cout << " factory update: " << this->factoryIface->data->newModel << std::endl;
+     std::string xmlString;
+     std::string xmlMiddle = (const char*)(this->factoryIface->data->newModel);
+ 
+@@ -121,6 +125,7 @@
+ 
+     strcpy((char*)this->factoryIface->data->deleteModel,"");
+   }
++  this->factoryIface->Unlock();
+ 
+ }
+ 
 Index: server/controllers/Controller.cc
 ===================================================================
 --- server/controllers/Controller.cc	(revision 7168)
@@ -3300,38 +3344,6 @@
    for (iter=this->ifaces.begin(); iter!=this->ifaces.end(); iter++)
    {
      if ((*iter)->GetOpenCount() > 0)
-Index: server/controllers/factory/Factory.cc
-===================================================================
---- server/controllers/factory/Factory.cc	(revision 7168)
-+++ server/controllers/factory/Factory.cc	(working copy)
-@@ -76,6 +76,8 @@
- // Initialize the controller
- void Factory::InitChild()
- {
-+  // initialize newModel to blank
-+  strcpy((char*)this->factoryIface->data->newModel,"");
- }
- 
- ////////////////////////////////////////////////////////////////////////////////
-@@ -83,8 +85,10 @@
- void Factory::UpdateChild()
- {
-   // If there is a string, then add the contents to the world
-+  this->factoryIface->Lock(1);
-   if (strcmp((const char*)this->factoryIface->data->newModel,"")!=0)
-   {
-+    //std::cout << " factory update: " << this->factoryIface->data->newModel << std::endl;
-     std::string xmlString;
-     std::string xmlMiddle = (const char*)(this->factoryIface->data->newModel);
- 
-@@ -121,6 +125,7 @@
- 
-     strcpy((char*)this->factoryIface->data->deleteModel,"");
-   }
-+  this->factoryIface->Unlock();
- 
- }
- 
 Index: server/controllers/ptz/generic/Generic_PTZ.cc
 ===================================================================
 --- server/controllers/ptz/generic/Generic_PTZ.cc	(revision 7168)

Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/test_slide.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/test_slide.xml	2008-12-17 00:28:07 UTC (rev 8218)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/test_slide.xml	2008-12-17 00:28:15 UTC (rev 8219)
@@ -1,7 +1,7 @@
 <launch>
   <master auto="start" />
 
-    <node pkg="gazebo" type="gazebo" args="-r $(find gazebo_robot_description)/gazebo_worlds/test/test_slide.world" respawn="false" output="screen">
+    <node pkg="gazebo" type="gazebo" args="-n $(find gazebo_robot_description)/gazebo_worlds/test/test_slide.world" respawn="false" output="screen">
       <env name="LD_LIBRARY_PATH" value="$(find gazebo_plugin)/lib:$(find gazebo)/gazebo/lib:$(find Cg)/Cg/lib:$(find boost)/boost/lib" />
       <env name="GAZEBO_RESOURCE_PATH" value="$(find gazebo_robot_description)/world" />
       <env name="OGRE_RESOURCE_PATH" value="$(find ogre)/ogre/lib/OGRE" />
@@ -12,7 +12,7 @@
     <param name="robotdesc/pr2" command="$(find xacro)/xacro.py '$(find wg_robot_description)/pr2/pr2.xacro.xml'" />
 
     <!-- push robotdesc/pr2 to factory and spawn robot in gazebo -->
-    <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 0 8 110 0 0 90" respawn="false" output="screen" /> <!-- load default arm controller -->
+    <node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2 0 8 90 0 0 90" respawn="false" output="screen" /> <!-- load default arm controller -->
 
     <!-- load controllers -->
     <include file="$(find pr2_gazebo)/pr2_default_controllers.launch" />

Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_camera.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_camera.world	2008-12-17 00:28:07 UTC (rev 8218)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_camera.world	2008-12-17 00:28:15 UTC (rev 8219)
@@ -25,7 +25,7 @@
   <!-- erp is typically .1-.8 -->
   <!-- here's the global contact cfm/erp -->
   <physics:ode>
-    <stepTime>0.0005</stepTime>
+    <stepTime>0.001</stepTime>
     <gravity>0 0 -9.8</gravity>
     <cfm>0.0000000001</cfm>
     <erp>0.2</erp>

Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_scan.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_scan.world	2008-12-17 00:28:07 UTC (rev 8218)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_scan.world	2008-12-17 00:28:15 UTC (rev 8219)
@@ -24,7 +24,7 @@
   <!-- erp is typically .1-.8 -->
   <!-- here's the global contact cfm/erp -->
   <physics:ode>
-    <stepTime>0.0005</stepTime>
+    <stepTime>0.001</stepTime>
     <gravity>0 0 -9.8</gravity>
     <cfm>0.0000000001</cfm>
     <erp>0.2</erp>

Modified: pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_slide.world
===================================================================
--- pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_slide.world	2008-12-17 00:28:07 UTC (rev 8218)
+++ pkg/trunk/robot_descriptions/gazebo_robot_description/gazebo_worlds/test/test_slide.world	2008-12-17 00:28:15 UTC (rev 8219)
@@ -24,7 +24,7 @@
   <!-- erp is typically .1-.8 -->
   <!-- here's the global contact cfm/erp -->
   <physics:ode>
-    <stepTime>0.0005</stepTime>
+    <stepTime>0.001</stepTime>
     <gravity>0 0 -9.8</gravity>
     <cfm>0.0000000001</cfm>
     <erp>0.2</erp>


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.