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

[<stu...@us...>]

Revision: 4792
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4792&view=rev
Author:   stuglaser
Date:     2008-09-29 23:11:37 +0000 (Mon, 29 Sep 2008)

Log Message:
-----------
The calibration controller now works consistently.

Modified Paths:
--------------
    pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
    pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
    pkg/trunk/manip/teleop_robot/scripts/calibrate.py
    pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h

Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h	2008-09-29 23:11:18 UTC (rev 4791)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/joint_calibration_controller.h	2008-09-29 23:11:37 UTC (rev 4792)
@@ -43,6 +43,17 @@
     This class moves the joint and reads the value of the clibration_reading_ field to find the zero position of the joint. Once these are determined, these values
  * are passed to the joint and enable the joint for the other controllers.
 
+ Example XML:
+ <controller type="JointCalibrationController">
+   <calibrate joint="upperarm_roll_right_joint"
+              actuator="upperarm_roll_right_act"
+              transmission="upperarm_roll_right_trans"
+              velocity="0.3" />
+   <pid p="15" i="0" d="0" iClamp="0" />
+ </controller>
+
+
+
 */
 /***************************************************/
 
@@ -56,7 +67,7 @@
 namespace controller
 {
 
-class JointCalibrationController : public JointManualCalibrationController
+class JointCalibrationController : public controller::Controller
 {
 public:
   /*!
@@ -82,15 +93,26 @@
    */
   virtual void update();
 
+  bool calibrated() { return state_ == STOPPED; }
+  void beginCalibration()
+  {
+    if (state_ == INITIALIZED)
+      state_ = BEGINNING;
+  }
+
 protected:
 
-  double previous_reading_; /**Previous calibration reading*/
+  enum { INITIALIZED, BEGINNING, MOVING, STOPPED };
+  int state_;
 
   double search_velocity_;
+  bool original_switch_state_;
 
-  double velocity_cmd_;
+  Actuator *actuator_;
+  mechanism::JointState *joint_;
+  mechanism::Transmission *transmission_;
 
-  controller::JointVelocityController vcontroller_; /** The joint velocity controller used to sweep the joint.*/
+  controller::JointVelocityController vc_;
 };
 
 

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp	2008-09-29 23:11:18 UTC (rev 4791)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/joint_calibration_controller.cpp	2008-09-29 23:11:37 UTC (rev 4792)
@@ -41,7 +41,7 @@
 ROS_REGISTER_CONTROLLER(JointCalibrationController)
 
 JointCalibrationController::JointCalibrationController()
-  : JointManualCalibrationController()
+: state_(INITIALIZED)
 {
 }
 
@@ -53,78 +53,110 @@
 {
   assert(robot);
   assert(config);
-  robot_ = robot->model_;
-  bool base=JointManualCalibrationController::initXml(robot, config);
-  if(!base)
-    return false;
-  TiXmlElement *j = config->FirstChildElement("param");
-  if (!j)
+
+  TiXmlElement *cal = config->FirstChildElement("calibrate");
+  if (!cal)
   {
-    std::cerr<<"JointCalibrationController was not given parameters"<<std::endl;
+    std::cerr<<"JointCalibrationController was not given calibration parameters"<<std::endl;
     return false;
   }
 
-  if(j->QueryDoubleAttribute("velocity", &search_velocity_ ) != TIXML_SUCCESS)
+  if(cal->QueryDoubleAttribute("velocity", &search_velocity_) != TIXML_SUCCESS)
   {
     std::cerr<<"Velocity value was not specified\n";
     return false;
   }
 
-  if(search_velocity_ == 0)
+  const char *joint_name = cal->Attribute("joint");
+  joint_ = joint_name ? robot->getJointState(joint_name) : NULL;
+  if (!joint_)
   {
-    std::cerr<<"You gave zero velocity\n";
+    fprintf(stderr, "Error: JointCalibrationController could not find joint \"%s\"\n",
+            joint_name);
     return false;
   }
 
-  TiXmlElement *v = config->FirstChildElement("controller");
-  if(!v)
+  const char *actuator_name = cal->Attribute("actuator");
+  actuator_ = actuator_name ? robot->model_->getActuator(actuator_name) : NULL;
+  if (!actuator_)
   {
-    std::cerr<<"JointCalibrationController was not given a controller to move the joint."<<std::endl;
+    fprintf(stderr, "Error: JointCalibrationController could not find actuator \"%s\"\n",
+            actuator_name);
     return false;
   }
 
-  return vcontroller_.initXml(robot, v);
+  const char *transmission_name = cal->Attribute("transmission");
+  transmission_ = transmission_name ? robot->model_->getTransmission(transmission_name) : NULL;
+  if (!transmission_)
+  {
+    fprintf(stderr, "Error: JointCalibrationController could not find transmission \"%s\"\n",
+            transmission_name);
+    return false;
+  }
+
+  control_toolbox::Pid pid;
+  TiXmlElement *pid_el = config->FirstChildElement("pid");
+  if (!pid_el)
+  {
+    fprintf(stderr, "Error: JointCalibrationController was not given a pid element.\n");
+    return false;
+  }
+  if (!pid.initXml(pid_el))
+    return false;
+
+  if (!vc_.init(robot, joint_name, pid))
+    return false;
+
+  return true;
 }
 
 void JointCalibrationController::update()
 {
-  assert(joint_state_);
+  assert(joint_);
   assert(actuator_);
 
-  const double cur_reading = actuator_->state_.calibration_reading_;
-  if(state_ == Begin)
+  switch(state_)
   {
-    joint_state_->calibrated_ = false;
-    previous_reading_ = cur_reading;
-    velocity_cmd_ = cur_reading > 0.5 ? search_velocity_ : -search_velocity_;
-    state_ = Search;
-  }
+  case INITIALIZED:
+    vc_.setCommand(0.0);
+    break;
+  case BEGINNING:
+    original_switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
+    vc_.setCommand(original_switch_state_ ? search_velocity_ : -search_velocity_);
+    state_ = MOVING;
+    break;
+  case MOVING: {
+    bool switch_state_ = actuator_->state_.calibration_reading_ > 0.5;
+    if (switch_state_ != original_switch_state_)
+    {
+      std::vector<Actuator*> fake_a;
+      std::vector<mechanism::JointState*> fake_j;
+      fake_a.push_back(new Actuator);
+      fake_j.push_back(new mechanism::JointState);
 
-  if(std::abs(cur_reading-previous_reading_)>0.5 && state_==Search)
-  {
-    const double offset_ = offset(actuator_->state_.position_, joint_state_->joint_->reference_position_);
-    actuator_->state_.zero_offset_ = offset_;
-    state_ = Initialized;
-  }
+      // Where was the joint when the optical switch triggered?
+      if (original_switch_state_ = true)
+        fake_a[0]->state_.position_ = actuator_->state_.last_calibration_high_transition_;
+      else
+        fake_a[0]->state_.position_ = actuator_->state_.last_calibration_low_transition_;
+      transmission_->propagatePosition(fake_a, fake_j);
 
-  if(state_ == Initialized)
-  {
-    joint_state_->calibrated_ = true;
-    velocity_cmd_ = 0;
-    state_ = Stop;
-    state_mutex_.unlock();
-  }
+      // What is the actuator position at the joint's zero?
+      fake_j[0]->position_ = fake_j[0]->position_ - joint_->joint_->reference_position_;
+      transmission_->propagatePositionBackwards(fake_j, fake_a);
 
-  if(state_ == Stop)
-  {
-    velocity_cmd_ = 0;
+      actuator_->state_.zero_offset_ = fake_a[0]->state_.position_;
+
+      state_ = STOPPED;
+    }
+    break;
   }
+  case STOPPED:
+    vc_.setCommand(0.0);
+    break;
+  }
 
-  if(state_!=Stop)
-  {
-    vcontroller_.setCommand(velocity_cmd_);
-    vcontroller_.update();
-  }
+  vc_.update();
 }
 
 
@@ -152,7 +184,7 @@
   ros::Duration d=ros::Duration(0,1000000);
   while(!c_->calibrated())
     d.sleep();
-  c_->getOffset(resp.offset);
+  resp.offset = 0;
   return true;
 }
 

Modified: pkg/trunk/manip/teleop_robot/scripts/calibrate.py
===================================================================
--- pkg/trunk/manip/teleop_robot/scripts/calibrate.py	2008-09-29 23:11:18 UTC (rev 4791)
+++ pkg/trunk/manip/teleop_robot/scripts/calibrate.py	2008-09-29 23:11:37 UTC (rev 4792)
@@ -92,15 +92,33 @@
     kill_controller(name)
     print "Calibrated"
 
-calibrate_optically(slurp('pr2_arm/calibration_shoulder_pan.xml'))
-calibrate_optically(slurp('pr2_arm/calibration_shoulder_lift.xml'))
 
+calibrate_optically('''
+<controller name="cal_shoulder_pan" topic="cal_shoulder_pan" type="JointCalibrationControllerNode">
+  <calibrate joint="shoulder_pan_right_joint"
+             actuator="shoulder_pan_right_act"
+             transmission="shoulder_pan_right_trans"
+             velocity="0.6" />
+  <pid p="7" i="0.5" d="0" iClamp="1.0" />
+</controller>
+''')
+
+calibrate_optically('''
+<controller name="cal_shoulder_pitch" topic="cal_shoulder_pitch" type="JointCalibrationControllerNode">
+  <calibrate joint="shoulder_pitch_right_joint"
+             actuator="shoulder_lift_right_act"
+             transmission="shoulder_lift_right_trans"
+             velocity="0.6" />
+  <pid p="7" i="0.5" d="0" iClamp="1.0" />
+</controller>
+''')
+
 calibrate_blindly('''
 <controller name="upperarm_calibration" topic="upperarm_calibration" type="JointBlindCalibrationControllerNode">
   <calibrate joint="upperarm_roll_right_joint"
              actuator="upperarm_roll_right_act"
              transmission="upperarm_roll_right_trans"
-             velocity="0.6" />
+             velocity="0.9" />
   <pid p="5" i="0.5" d="0" iClamp="1.0" />
 </controller>
 
@@ -111,7 +129,7 @@
   <calibrate joint="elbow_right_joint"
              actuator="elbow_right_act"
              transmission="elbow_right_trans"
-             velocity="0.6" />
+             velocity="0.8" />
   <pid p="5" i="0.5" d="0" iClamp="1.0" />
 </controller>
 

Modified: pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h
===================================================================
--- pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h	2008-09-29 23:11:18 UTC (rev 4791)
+++ pkg/trunk/mechanism/hardware_interface/include/hardware_interface/hardware_interface.h	2008-09-29 23:11:37 UTC (rev 4792)
@@ -63,7 +63,7 @@
   double velocity_;
 
   bool calibration_reading_;
-  int32_t last_calibration_high_transition_;
+  int32_t last_calibration_high_transition_;  // Last transition from high to low.
   int32_t last_calibration_low_transition_;
 
   bool is_enabled_;


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