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

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

Revision: 816
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=816&view=rev
Author:   hsujohnhsu
Date:     2008-06-14 13:51:13 -0700 (Sat, 14 Jun 2008)

Log Message:
-----------
steering and moving separated.  this seems to fix the 2dnav-gazebo nav problem for now.

Modified Paths:
--------------
    pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
    pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
    pkg/trunk/simulators/rosgazebo/rosgazebo.cc

Modified: pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h	2008-06-14 02:20:29 UTC (rev 815)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/include/libpr2API/pr2API.h	2008-06-14 20:51:13 UTC (rev 816)
@@ -563,7 +563,7 @@
          /*! \fn
            \brief Checks to see if steering angles are lined up
          */
-      public: bool CheckBaseSteeringAngle(double errorTol);
+      public: double BaseSteeringAngleError();
 
          /*! \fn
            \brief Retrieve commanded speed for the base in cartesian space in body coordinates 

Modified: pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp
===================================================================
--- pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp	2008-06-14 02:20:29 UTC (rev 815)
+++ pkg/trunk/drivers/robot/pr2/libpr2API/src/pr2API.cpp	2008-06-14 20:51:13 UTC (rev 816)
@@ -87,6 +87,68 @@
    return sqrt(x*x+y*y);
 }
 
+
+/* --------------- utility functions ---------------- */
+/* ---------------        TODO       ---------------- */
+/* --------------- move to a library ---------------- */
+
+#define FROM_DEGREES(degrees)	(((double)(degrees))/180*M_PI)
+/*
+ * normalize_angle_positive
+ *
+ * Normalizes the angle to be 0 circle to 1 circle
+ * It takes and returns native units.
+ */
+
+double normalize_angle_positive(double angle)
+{
+  return fmod(fmod(angle, FROM_DEGREES(360))+FROM_DEGREES(360), FROM_DEGREES(360));
+}
+/*
+ * normalize
+ *
+ * Normalizes the angle to be -1/2 circle to +1/2 circle
+ * It takes and returns native units.
+ *
+ */
+
+double normalize_angle(double angle)
+{
+  double a=normalize_angle_positive(angle);
+  if (a>FROM_DEGREES(180))
+      a-=FROM_DEGREES(360);
+  return(a);
+}
+/*
+ * shortest_angular_distance
+ *
+ * Given 2 angles, this returns the shortest angular
+ * difference.  The inputs and ouputs are of course native
+ * units.
+ *
+ * As an example, if native units are degrees, the result
+ * would always be -180 <= result <= 180.  Adding the result
+ * to "from" will always get you an equivelent angle to "to".
+ */
+
+double shortest_angular_distance(double from, double to)
+{
+  double result;
+  result=normalize_angle_positive(
+      normalize_angle_positive(to) - normalize_angle_positive(from));
+
+  if ( result > FROM_DEGREES(180) ) {  // If the result > 180,
+                                       // It's shorter the other way.
+      result=-(FROM_DEGREES(360)-result);   
+  }
+  return normalize_angle(result);
+}
+
+
+
+
+
+
 PR2Robot::PR2Robot()
 {
 };
@@ -1297,7 +1359,7 @@
    return PR2_ALL_OK;
 };
 
-bool PR2Robot::CheckBaseSteeringAngle(double errorTol)
+double PR2Robot::BaseSteeringAngleError()
 {
    point steerPointVelocity[NUM_CASTERS];
    double steerAngle[NUM_CASTERS];
@@ -1319,15 +1381,14 @@
      // this is a goofy hack, should put a more complicated version later
      GetJointServoCmd((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),&currentCmd[ii],&currentRateCmd[ii]);
      GetJointServoActual((PR2_JOINT_ID)(CASTER_FL_STEER+3*ii),&currentAngle[ii],&currentRateAct[ii]);
-     currentError[ii] = fabs(currentCmd[ii] - currentAngle[ii]);
-     //std::cout << " e " << ii << " e " << currentError[ii] << std::endl;
+     currentError[ii] = fabs(shortest_angular_distance(currentCmd[ii] , currentAngle[ii]));
+     //std::cout << " ii " << ii << " e " << currentError[ii] << std::endl;
+     fprintf(stdout," ii %d e %.3f \n", ii , currentError[ii]);
      errorTotal = errorTotal + currentError[ii];
    }
+   fprintf(stdout," ----------------------------------\n");
 
-   if (errorTotal < errorTol)
-     return true;
-   else
-     return false;
+   return errorTotal;
 };
 
 

Modified: pkg/trunk/simulators/rosgazebo/rosgazebo.cc
===================================================================
--- pkg/trunk/simulators/rosgazebo/rosgazebo.cc	2008-06-14 02:20:29 UTC (rev 815)
+++ pkg/trunk/simulators/rosgazebo/rosgazebo.cc	2008-06-14 20:51:13 UTC (rev 816)
@@ -196,39 +196,37 @@
   this->lock.lock();
   double dt;
   double w11, w21, w12, w22;
-  w11 = 1.0;
-  w21 = 1.0;
-  w12 = 1.0;
-  w22 = 1.0;
 
   // smooth out the commands by time decay
   // with w1,w2=1, this means equal weighting for new command every second
   this->myPR2->GetSimTime(&(this->simTime));
   dt = simTime - lastTime;
-  //this->vxSmooth = (w11 * this->vxSmooth + w21*dt *this->velMsg.vx)/( w11 + w21*dt);
-  //this->vwSmooth = (w12 * this->vwSmooth + w22*dt *this->velMsg.vw)/( w12 + w22*dt);
 
-  this->vxSmooth = this->velMsg.vx;
-  this->vwSmooth = this->velMsg.vw;
+  // smooth if dt is larger than zero
+  if (dt > 0.0)
+  {
+    w11 =  0.0;
+    w21 =  1.0;
+    w12 =  0.0;
+    w22 =  1.0;
+    this->vxSmooth = (w11 * this->vxSmooth + w21*dt *this->velMsg.vx)/( w11 + w21*dt);
+    this->vwSmooth = (w12 * this->vwSmooth + w22*dt *this->velMsg.vw)/( w12 + w22*dt);
+  }
 
-  fprintf(stderr,"received cmd: %.3f %.3f | %.3f %.3f\n", this->velMsg.vx, this->velMsg.vw,this->vxSmooth, this->vwSmooth);
-
-  this->myPR2->SetBaseSteeringAngle    (this->vxSmooth,0.0,this->vwSmooth);
-  while (!this->myPR2->CheckBaseSteeringAngle(M_PI/10.0))
+  // if steering angle is wrong, don't move or move slowly
+  std::cout << "received cmd: vx " << this->velMsg.vx << " vw " <<  this->velMsg.vw
+            << " vxsmoo " << this->vxSmooth << " vxsmoo " <<  this->vwSmooth
+            << " | steer erros: " << this->myPR2->BaseSteeringAngleError() << " - " <<  M_PI/100.0
+            << std::endl;
+  if (this->myPR2->BaseSteeringAngleError() > M_PI/100.0)
   {
-    // do nothing and wait...
-    usleep(100000);
+    this->myPR2->SetBaseSteeringAngle    (this->vxSmooth,0.0,this->vwSmooth);
   }
-  // set base velocity
-  this->myPR2->SetJointTorque(PR2::CASTER_FL_DRIVE_L, 1000.0 );
-  this->myPR2->SetJointTorque(PR2::CASTER_FR_DRIVE_L, 1000.0 );
-  this->myPR2->SetJointTorque(PR2::CASTER_RL_DRIVE_L, 1000.0 );
-  this->myPR2->SetJointTorque(PR2::CASTER_RR_DRIVE_L, 1000.0 );
-  this->myPR2->SetJointTorque(PR2::CASTER_FL_DRIVE_R, 1000.0 );
-  this->myPR2->SetJointTorque(PR2::CASTER_FR_DRIVE_R, 1000.0 );
-  this->myPR2->SetJointTorque(PR2::CASTER_RL_DRIVE_R, 1000.0 );
-  this->myPR2->SetJointTorque(PR2::CASTER_RR_DRIVE_R, 1000.0 );
-  this->myPR2->SetBaseCartesianSpeedCmd(this->vxSmooth, 0.0, this->vwSmooth);
+  else
+  {
+    // set base velocity
+    this->myPR2->SetBaseCartesianSpeedCmd(this->vxSmooth, 0.0, this->vwSmooth);
+  }
 
   this->lastTime = this->simTime;
 
@@ -265,6 +263,16 @@
 
   this->myPR2->GetSimTime(&(this->lastTime));
   this->myPR2->GetSimTime(&(this->simTime));
+
+  // set torques for driving the robot wheels
+  this->myPR2->SetJointTorque(PR2::CASTER_FL_DRIVE_L, 1000.0 );
+  this->myPR2->SetJointTorque(PR2::CASTER_FR_DRIVE_L, 1000.0 );
+  this->myPR2->SetJointTorque(PR2::CASTER_RL_DRIVE_L, 1000.0 );
+  this->myPR2->SetJointTorque(PR2::CASTER_RR_DRIVE_L, 1000.0 );
+  this->myPR2->SetJointTorque(PR2::CASTER_FL_DRIVE_R, 1000.0 );
+  this->myPR2->SetJointTorque(PR2::CASTER_FR_DRIVE_R, 1000.0 );
+  this->myPR2->SetJointTorque(PR2::CASTER_RL_DRIVE_R, 1000.0 );
+  this->myPR2->SetJointTorque(PR2::CASTER_RR_DRIVE_R, 1000.0 );
 }
 
 void GazeboNode::finalize(int)


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