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

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

Revision: 4814
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=4814&view=rev
Author:   stuglaser
Date:     2008-09-30 18:27:27 +0000 (Tue, 30 Sep 2008)

Log Message:
-----------
CartesianPositionController operates in vector space now.

Modified Paths:
--------------
    pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h
    pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h
    pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
    pkg/trunk/controllers/robot_mechanism_controllers/scripts/control.py
    pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp
    pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp
    pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py
    pkg/trunk/mechanism/mechanism_control/scripts/mech.py

Added Paths:
-----------
    pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv

Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h	2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_effort_controller.h	2008-09-30 18:27:27 UTC (rev 4814)
@@ -31,7 +31,7 @@
  * Example config:
 
  <controller type="CartesianEffortController" name="controller_name">
-   <chain root="root_link" tip="tip_link" />
+   <chain root="root_link" tip="tip_link" offset="0.3 0.1 0.2" />
  </controller>
 
  * The root is fixed, and all commands are specified in its coordinate
@@ -62,6 +62,7 @@
 
   btVector3 command_;
 
+  btVector3 offset_;
   std::vector<mechanism::LinkState*> links_;  // root to tip
   std::vector<mechanism::JointState*> joints_;  // root to tip, 1 element smaller than links_
 };

Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h	2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_position_controller.h	2008-09-30 18:27:27 UTC (rev 4814)
@@ -47,6 +47,7 @@
 #include <vector>
 #include "ros/node.h"
 #include "robot_mechanism_controllers/SetVectorCommand.h"
+#include "robot_mechanism_controllers/GetVector.h"
 #include "robot_mechanism_controllers/cartesian_effort_controller.h"
 #include "control_toolbox/pid.h"
 #include "mechanism_model/controller.h"
@@ -65,12 +66,13 @@
   void update();
 
   btVector3 command_;
+  void getTipPosition(btVector3 *p);
 
 private:
   mechanism::RobotState *robot_;
   mechanism::LinkState *tip_;
   CartesianEffortController effort_;
-  control_toolbox::Pid pid_;
+  control_toolbox::Pid pid_x_, pid_y_, pid_z_;
   double last_time_;
 
   bool reset_;
@@ -87,10 +89,12 @@
 
   bool setCommand(robot_mechanism_controllers::SetVectorCommand::request &req,
                   robot_mechanism_controllers::SetVectorCommand::response &resp);
+  bool getActual(robot_mechanism_controllers::GetVector::request &req,
+                 robot_mechanism_controllers::GetVector::response &resp);
 
 private:
   CartesianPositionController c_;
-  AdvertisedServiceGuard guard_set_actual_;
+  AdvertisedServiceGuard guard_set_command_, guard_get_actual_;
 };
 
 }

Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml	2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml	2008-09-30 18:27:27 UTC (rev 4814)
@@ -1,7 +1,7 @@
 <package>
   <description brief="Generic Mechanism Controller Library">
   </description>
-  <author>John Hsu, David Li, Melonee Wise</author>
+  <author>John Hsu, David Li, Melonee Wise, Stuart Glaser</author>
   <license>BSD</license>
   <depend package="mechanism_model" />
   <depend package="control_toolbox" />
@@ -10,6 +10,8 @@
   <depend package="misc_utils" />
   <depend package="roscpp" />
   <depend package="bullet" />
+  <depend package="std_msgs" />
+  <depend package="tf" />
   <url>http://pr.willowgarage.com</url>
   <repository>http://pr.willowgarage.com/repos</repository>
   <export>

Modified: pkg/trunk/controllers/robot_mechanism_controllers/scripts/control.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/scripts/control.py	2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/scripts/control.py	2008-09-30 18:27:27 UTC (rev 4814)
@@ -16,6 +16,7 @@
     set <controller> <command> - Set the controller's commanded value
     setv <controller> <x> <y> <z>  - Set the controller's command as a vector
     get <controller>           - Get the controller's commanded value
+    getv <controller>          - Get the controller's vector value
     profile <controller> <upper turnaround offset> <lower turnaround offset> <upper decel buffer> <lower decel bufer>
                                - Define how far away from joint limit to turn around. Buffers indicate how far from that point to start decelerating. Set to 0 to disable'''
     sys.exit(exit_code)
@@ -37,6 +38,10 @@
         if len(sys.argv) < 3:
           print_usage()
         controllers.get_controller(sys.argv[2])
+    elif sys.argv[1] == 'getv':
+        if len(sys.argv) < 3:
+            print_usage()
+        controllers.get_controller_vector(sys.argv[2])
     elif sys.argv[1] == 'profile':
         controllers.set_profile(sys.argv[2],float(sys.argv[3]),float(sys.argv[4]), float(sys.argv[5]), float(sys.argv[6]))
     elif sys.argv[1] == 'setPosition':

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp	2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_effort_controller.cpp	2008-09-30 18:27:27 UTC (rev 4814)
@@ -32,6 +32,7 @@
  */
 
 #include "robot_mechanism_controllers/cartesian_effort_controller.h"
+#include "urdf/parser.h"
 #include <algorithm>
 
 namespace controller {
@@ -40,6 +41,7 @@
 
 CartesianEffortController::CartesianEffortController()
 : command_(0,0,0),
+  offset_(0,0,0),
   links_(0,(mechanism::LinkState*)NULL),
   joints_(0,(mechanism::JointState*)NULL)
 {
@@ -111,6 +113,17 @@
 
   assert(joints_.size() == links_.size() - 1);
 
+  if (chain->Attribute("offset"))
+  {
+    std::vector<double> offset_pieces;
+    urdf::queryVectorAttribute(chain, "offset", &offset_pieces);
+    assert(offset_pieces.size() == 3);  // TODO
+
+    offset_[0] = offset_pieces[0];
+    offset_[1] = offset_pieces[1];
+    offset_[2] = offset_pieces[2];
+  }
+
   return true;
 }
 
@@ -129,8 +142,7 @@
   F.setValue(tempF.x, tempF.y, tempF.z);
   // At this point, F is the desired force in the current link's frame
 
-  // TODO: actual tip offset, not a made-up offset
-  btVector3 r(0.1,0,0);  // position of the force in the current frame
+  btVector3 r(offset_);  // position of the force in the current frame
 
   for (int i = links_.size() - 2; i >= 0; --i)
   {

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp	2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_position_controller.cpp	2008-09-30 18:27:27 UTC (rev 4814)
@@ -33,6 +33,7 @@
 
 #include "robot_mechanism_controllers/cartesian_position_controller.h"
 #include <algorithm>
+#include "tf/transform_datatypes.h"
 
 namespace controller {
 
@@ -63,8 +64,10 @@
     fprintf(stderr, "Error: CartesianPositionController requires a pid element\n");
     return false;
   }
-  if (!pid_.initXml(pid_el))
+  if (!pid_x_.initXml(pid_el))
     return false;
+  pid_y_ = pid_x_;
+  pid_z_ = pid_x_;
 
   last_time_ = robot_->hw_->current_time_;
 
@@ -75,24 +78,29 @@
 {
   if (reset_) {
     reset_ = false;
-    command_ = tip_->abs_position_;
+    command_ = tip_->abs_position_ + effort_.offset_;
   }
 
   assert(tip_);
   double time = robot_->hw_->current_time_;
 
+  btVector3 error = command_ - (tip_->abs_position_ + effort_.offset_);
+  effort_.command_[0] = -pid_x_.updatePid(error.x(), time - last_time_);
+  effort_.command_[1] = -pid_y_.updatePid(error.y(), time - last_time_);
+  effort_.command_[2] = -pid_z_.updatePid(error.z(), time - last_time_);
 
-  btVector3 v = command_ - tip_->abs_position_;
-  double error = v.length();
-  if (error > 0.0)
-    v.normalize();
-
-  effort_.command_ = v * -pid_.updatePid(error, time - last_time_);
+  printf("    %.4lf %.4lf %.4lf\n", effort_.command_[0],effort_.command_[1],effort_.command_[2]);
   effort_.update();
 
   last_time_ = time;
 }
 
+void CartesianPositionController::getTipPosition(btVector3 *p)
+{
+  *p = tip_->abs_position_ + effort_.offset_;
+}
+
+
 ROS_REGISTER_CONTROLLER(CartesianPositionControllerNode)
 
 CartesianPositionControllerNode::CartesianPositionControllerNode()
@@ -119,7 +127,10 @@
 
   node->advertise_service(topic + "/set_command",
                           &CartesianPositionControllerNode::setCommand, this);
-  guard_set_actual_.set(topic + "/set_command");
+  guard_set_command_.set(topic + "/set_command");
+  node->advertise_service(topic + "/get_actual",
+                          &CartesianPositionControllerNode::getActual, this);
+  guard_get_actual_.set(topic + "/get_actual");
   return true;
 }
 
@@ -136,5 +147,14 @@
   return true;
 }
 
+bool CartesianPositionControllerNode::getActual(
+  robot_mechanism_controllers::GetVector::request &req,
+  robot_mechanism_controllers::GetVector::response &resp)
+{
+  btVector3 v;
+  c_.getTipPosition(&v);
+  tf::Vector3TFToMsg(v, resp.v);
+  return true;
+}
 
 }

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py	2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/robot_mechanism_controllers/controllers.py	2008-09-30 18:27:27 UTC (rev 4814)
@@ -33,6 +33,12 @@
     resp = s.call(GetActualRequest())
     print str(resp.time) + ": " + str(resp.command)
 
+def get_controller_vector(controller):
+    rospy.wait_for_service(controller + '/get_actual')
+    s = rospy.ServiceProxy(controller + '/get_actual', GetVector)
+    resp = s()
+    print "(%f, %f, %f)" % (resp.v.x, resp.v.y, resp.v.z)
+
 def set_position(controller, command):
     rospy.wait_for_service(controller + '/set_position')
     s = rospy.ServiceProxy(controller + '/set_position', SetPosition)

Added: pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv	                        (rev 0)
+++ pkg/trunk/controllers/robot_mechanism_controllers/srv/GetVector.srv	2008-09-30 18:27:27 UTC (rev 4814)
@@ -0,0 +1,2 @@
+---
+std_msgs/Vector3 v
\ No newline at end of file

Modified: pkg/trunk/mechanism/mechanism_control/scripts/mech.py
===================================================================
--- pkg/trunk/mechanism/mechanism_control/scripts/mech.py	2008-09-30 18:13:14 UTC (rev 4813)
+++ pkg/trunk/mechanism/mechanism_control/scripts/mech.py	2008-09-30 18:27:27 UTC (rev 4814)
@@ -37,3 +37,5 @@
         mechanism.kill_controller(sys.argv[2])
     elif sys.argv[1] == 'shutdown':
         mechanism.shutdown()
+    else:
+        print_usage(1)


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