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

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

Revision: 15533
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=15533&view=rev
Author:   stuglaser
Date:     2009-05-15 18:49:59 +0000 (Fri, 15 May 2009)

Log Message:
-----------
Very slow, but accurate, plugging in, commanding velocities by leading the position command.

Modified Paths:
--------------
    pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
    pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
    pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h
    pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp
    pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_pose.yaml
    pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_twist.yaml

Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h	2009-05-15 18:46:26 UTC (rev 15532)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h	2009-05-15 18:49:59 UTC (rev 15533)
@@ -41,6 +41,7 @@
 #include <tf/message_notifier.h>
 #include "realtime_tools/realtime_publisher.h"
 #include "filters/filter_chain.h"
+#include "control_toolbox/pid_gains_setter.h"
 
 #include "robot_srvs/SetPoseStamped.h"
 #include "manipulation_msgs/TaskFrameFormalism.h"
@@ -119,6 +120,10 @@
   unsigned int loop_count_;
   boost::scoped_ptr<realtime_tools::RealtimePublisher<robot_mechanism_controllers::CartesianHybridState> > pub_state_;
   boost::scoped_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > pub_tf_;
+  control_toolbox::PidGainsSetter pose_pid_tuner_;
+  control_toolbox::PidGainsSetter pose_rot_pid_tuner_;
+  control_toolbox::PidGainsSetter twist_pid_tuner_;
+  control_toolbox::PidGainsSetter twist_rot_pid_tuner_;
 };
 
 }

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp	2009-05-15 18:46:26 UTC (rev 15532)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp	2009-05-15 18:49:59 UTC (rev 15533)
@@ -243,8 +243,14 @@
   {
     if (mode_[i] == manipulation_msgs::TaskFrameFormalism::POSITION)
       pose_desi_[i] = setpoint_[i];
+    else// if (mode_[i] == manipulation_msgs::TaskFrameFormalism::VELOCITY)
+    {
+      pose_desi_[i] += setpoint_[i] * dt;
+    }
+    /*
     else
       pose_desi_[i] = 0.0;
+    */
   }
 
   // Computes the pose error
@@ -265,7 +271,8 @@
       twist_desi_[i] = pose_pids_[i].updatePid(pose_error_[i], twist_meas_filtered_[i], dt);
       break;
     case manipulation_msgs::TaskFrameFormalism::VELOCITY:
-      twist_desi_[i] = setpoint_[i];
+      //twist_desi_[i] = setpoint_[i];
+      twist_desi_[i] = pose_pids_[i].updatePid(pose_error_[i], twist_meas_filtered_[i] - setpoint_[i], dt);
       break;
     }
   }
@@ -297,8 +304,17 @@
     switch (mode_[i])
     {
     case manipulation_msgs::TaskFrameFormalism::POSITION:
+      /*
+      if (i >= 3) {
+        wrench_desi_[i] = twist_desi_[i];
+        break;
+      }
+      */
+      wrench_desi_[i] = twist_desi_[i];
+      break;
     case manipulation_msgs::TaskFrameFormalism::VELOCITY:
-      wrench_desi_[i] = twist_pids_[i].updatePid(twist_error_[i], dt);
+      //wrench_desi_[i] = twist_pids_[i].updatePid(twist_error_[i], dt);
+      wrench_desi_[i] = twist_desi_[i];
       break;
     case manipulation_msgs::TaskFrameFormalism::FORCE:
       wrench_desi_[i] = setpoint_[i];
@@ -416,6 +432,19 @@
 
   node->advertiseService(name_ + "/set_tool_frame", &CartesianHybridControllerNode::setToolFrame, this);
 
+  for (int i = 0; i < 3; ++i)
+    pose_pid_tuner_.add(c_.pose_pids_ + i);
+  pose_pid_tuner_.advertise(name_ + "/pose");
+  for (int i = 3; i < 6; ++i)
+    pose_rot_pid_tuner_.add(c_.pose_pids_ + i);
+  pose_rot_pid_tuner_.advertise(name_ + "/pose_rot");
+  for (int i = 0; i < 3; ++i)
+    twist_pid_tuner_.add(c_.twist_pids_ + i);
+  twist_pid_tuner_.advertise(name_ + "/twist");
+  for (int i = 3; i < 6; ++i)
+    twist_rot_pid_tuner_.add(c_.twist_pids_ + i);
+  twist_rot_pid_tuner_.advertise(name_ + "/twist_rot");
+
   return true;
 
 }

Modified: pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h	2009-05-15 18:46:26 UTC (rev 15532)
+++ pkg/trunk/highlevel/plugs/plugs_core/include/plugs_core/action_plug_in.h	2009-05-15 18:49:59 UTC (rev 15533)
@@ -53,9 +53,6 @@
 #include <tf/message_notifier.h>
 #include "boost/scoped_ptr.hpp"
 
-//detecton
-#include "outlet_detection/plug_tracker.h"
-
 // Robot Action Stuff
 #include <robot_actions/action.h>
 
@@ -87,12 +84,20 @@
 
   std::string arm_controller_;
 
-  PlugTracker::PlugTracker* detector_;
   std_msgs::Empty empty_;
 
   boost::scoped_ptr<tf::MessageNotifier<robot_msgs::PoseStamped> > notifier_;
   boost::scoped_ptr<tf::TransformListener> TF_;
 
+
+  // TODO: mutex
+  ros::Time vision_estimate_time_;
+  tf::Pose outlet_pose_mech_;  // Outlet pose in the mechanism "frame"
+
+
+
+
+
   double last_standoff_;
   ros::Time g_started_inserting_, g_started_forcing_, g_stopped_forcing_;
   int g_state_;

Modified: pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp
===================================================================
--- pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp	2009-05-15 18:46:26 UTC (rev 15532)
+++ pkg/trunk/highlevel/plugs/plugs_core/src/action_plug_in.cpp	2009-05-15 18:49:59 UTC (rev 15533)
@@ -46,8 +46,7 @@
   robot_actions::Action<std_msgs::Empty, std_msgs::Empty>("plug_in"),
   action_name_("plug_in"),
   node_(node),
-  arm_controller_("r_arm_hybrid_controller"),
-  detector_(NULL)
+  arm_controller_("r_arm_hybrid_controller")
 {
   node_.setParam("~roi_policy", "LastImageLocation");
   node_.setParam("~display", 0);
@@ -66,9 +65,6 @@
 
   node_.advertise<manipulation_msgs::TaskFrameFormalism>(arm_controller_ + "/command", 2);
 
-  //detector_ = new PlugTracker::PlugTracker(node);
-  //detector_->deactivate();
-
   TF_.reset(new tf::TransformListener(node));
   notifier_.reset(new tf::MessageNotifier<robot_msgs::PoseStamped>(
                     TF_.get(), &node,
@@ -82,14 +78,11 @@
 
 PlugInAction::~PlugInAction()
 {
-  if(detector_) delete detector_;
 };
 
 robot_actions::ResultStatus PlugInAction::execute(const std_msgs::Empty& empty, std_msgs::Empty& feedback)
 {
   reset();
-  if (detector_)
-    detector_->activate();
 
   return waitForDeactivation(feedback);
 }
@@ -122,8 +115,6 @@
 
   if (isPreemptRequested()){
     deactivate(robot_actions::PREEMPTED, empty_);
-    if (detector_)
-      detector_->deactivate();
     return;
   }
   tff_msg_.header.stamp = msg->header.stamp;
@@ -202,7 +193,7 @@
     {
       tf::Vector3 offset = viz_offset.getOrigin() - viz_offset_desi.getOrigin();
       ROS_DEBUG("%s: Offset: (% 0.3lf, % 0.3lf, % 0.3lf)", action_name_.c_str(), offset.x(), offset.y(), offset.z());
-      if (g_started_inserting_ + ros::Duration(5.0) < ros::Time::now())
+      if (g_started_inserting_ + ros::Duration(30.0) < ros::Time::now())
       {
         if (offset.x() > SUCCESS_THRESHOLD) // if (in_outlet)
         {
@@ -265,8 +256,6 @@
         ROS_DEBUG("HOLDING");
         hold();
         deactivate(robot_actions::SUCCESS, empty_);
-        if (detector_)
-          detector_->deactivate();
         break;
       }
     }
@@ -332,7 +321,7 @@
   tff_msg_.mode.rot.x = 3;
   tff_msg_.mode.rot.y = 3;
   tff_msg_.mode.rot.z = 3;
-  tff_msg_.value.vel.x = 0.3;
+  tff_msg_.value.vel.x = 0.001;
   tff_msg_.value.vel.y = mech_offset_desi_.getOrigin().y();
   tff_msg_.value.vel.z = mech_offset_desi_.getOrigin().z();
   mech_offset_desi_.getBasis().getEulerZYX(tff_msg_.value.rot.z, tff_msg_.value.rot.y, tff_msg_.value.rot.x);

Modified: pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_pose.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_pose.yaml	2009-05-15 18:46:26 UTC (rev 15532)
+++ pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_pose.yaml	2009-05-15 18:49:59 UTC (rev 15533)
@@ -1,12 +1,12 @@
 fb_trans:
-  p: 60
-  i: 80
-  d: 0
-  i_clamp: 0.2
+  p: 200
+  i: 100
+  d: 0.15
+  i_clamp: 10
 fb_rot:
-  p: 10
+  p: 15
   i: 0
-  d: 0
-  i_clamp: 0.4
+  d: 0.15
+  i_clamp: 0.0
 ff_trans: 0.0
 ff_rot: 0.0

Modified: pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_twist.yaml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_twist.yaml	2009-05-15 18:46:26 UTC (rev 15532)
+++ pkg/trunk/robot_descriptions/pr2/pr2_default_controllers/gains/arm_twist.yaml	2009-05-15 18:49:59 UTC (rev 15533)
@@ -4,7 +4,8 @@
   d: 0.0
   i_clamp: 1.0
 fb_rot:
-  p: 1.5
+  p: 9999999991.5
+  _p: 1.5
   i: 0.0
   d: 0.0
   i_clamp: 0.2


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