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

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

Revision: 13985
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=13985&view=rev
Author:   stuglaser
Date:     2009-04-17 00:15:33 +0000 (Fri, 17 Apr 2009)

Log Message:
-----------
Updated to use the conversion methods in the tf_conversions package.

Modified Paths:
--------------
    pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
    pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
    pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
    pkg/trunk/mechanism/mechanism_model/include/mechanism_model/chain.h
    pkg/trunk/mechanism/mechanism_model/manifest.xml
    pkg/trunk/mechanism/mechanism_model/src/chain.cpp

Modified: pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml	2009-04-17 00:15:05 UTC (rev 13984)
+++ pkg/trunk/controllers/robot_mechanism_controllers/manifest.xml	2009-04-17 00:15:33 UTC (rev 13985)
@@ -16,6 +16,7 @@
   <depend package="robot_srvs" />
   <depend package="robot_msgs" />
   <depend package="tf" />
+  <depend package="tf_conversions" />
   <depend package="kdl" />
   <depend package="angles" />
   <depend package="joy" />

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp	2009-04-17 00:15:05 UTC (rev 13984)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp	2009-04-17 00:15:33 UTC (rev 13985)
@@ -37,6 +37,7 @@
 #include <kdl/chainfksolvervel_recursive.hpp>
 #include <kdl/chainjnttojacsolver.hpp>
 #include <kdl/jacobian.hpp>
+#include "tf_conversions/tf_kdl.h"
 #include "realtime_tools/realtime_publisher.h"
 #include "control_toolbox/pid.h"
 #include "robot_msgs/VisualizationMarker.h"
@@ -49,7 +50,7 @@
 void TransformKDLToMsg(const KDL::Frame &k, robot_msgs::Pose &m)
 {
   tf::Transform tf;
-  mechanism::TransformKDLToTF(k, tf);
+  tf::TransformKDLToTF(k, tf);
   tf::PoseTFToMsg(tf, m);
 }
 
@@ -443,7 +444,7 @@
       pub_tf_->msg_.transforms[0].header.frame_id = name_ + "/tool_frame";
       pub_tf_->msg_.transforms[0].parent_id = c_.chain_.getLinkName();
       tf::Transform t;
-      mechanism::TransformKDLToTF(c_.tool_frame_offset_, t);
+      tf::TransformKDLToTF(c_.tool_frame_offset_, t);
       tf::TransformTFToMsg(t, pub_tf_->msg_.transforms[0].transform);
       pub_tf_->unlockAndPublish();
     }
@@ -470,7 +471,7 @@
     ROS_WARN("Transform Exception %s", ex.what());
     return;
   }
-  mechanism::TransformTFToKDL(task_frame, c_.task_frame_offset_);
+  tf::TransformTFToKDL(task_frame, c_.task_frame_offset_);
 
   c_.mode_[0] = (int)command_msg_.mode.vel.x;
   c_.mode_[1] = (int)command_msg_.mode.vel.y;

Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp	2009-04-17 00:15:05 UTC (rev 13984)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp	2009-04-17 00:15:33 UTC (rev 13985)
@@ -34,6 +34,7 @@
 #include "urdf/parser.h"
 #include <algorithm>
 #include "robot_mechanism_controllers/plug_controller.h"
+#include "tf_conversions/tf_kdl.h"
 
 #define DEBUG 0 // easy debugging
 
@@ -72,9 +73,9 @@
 {
   assert(robot);
   robot_ = robot;
-  
+
   controller_name_ = config->Attribute("name");
-  
+
   TiXmlElement *chain = config->FirstChildElement("chain");
   if (!chain) {
     ROS_ERROR("PlugController was not given a chain");
@@ -108,13 +109,13 @@
   node->param(controller_name_+"/f_limit_max"  , f_limit_max  , 100.0) ; /// max upper arm limit force
   node->param(controller_name_+"/upper_arm_dead_zone", upper_arm_dead_zone, 0.05);
 
-  roll_pid_.initParam(controller_name_+"/pose_pid"); 
-  pitch_pid_.initParam(controller_name_+"/pose_pid"); 
-  yaw_pid_.initParam(controller_name_+"/pose_pid"); 
-  line_pid_.initParam(controller_name_+"/line_pid"); 
+  roll_pid_.initParam(controller_name_+"/pose_pid");
+  pitch_pid_.initParam(controller_name_+"/pose_pid");
+  yaw_pid_.initParam(controller_name_+"/pose_pid");
+  line_pid_.initParam(controller_name_+"/line_pid");
   last_time_ = robot->model_->hw_->current_time_;
-  
-  
+
+
   // Constructs solvers and allocates matrices.
   unsigned int num_joints   = kdl_chain_.getNrOfJoints();
   jnt_to_jac_solver_.reset(new ChainJntToJacSolver(kdl_chain_));
@@ -335,7 +336,7 @@
   chain_.toKDL(new_kdl_chain);
 
   KDL::Frame tool_frame;
-  mechanism::TransformTFToKDL(tool_offset, tool_frame);
+  tf::TransformTFToKDL(tool_offset, tool_frame);
   new_kdl_chain.addSegment(KDL::Segment(KDL::Joint(KDL::Joint::None), tool_frame));
 
   kdl_chain_ = new_kdl_chain;
@@ -391,7 +392,7 @@
   if (current_frame_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
     delete current_frame_publisher_ ;
   current_frame_publisher_ = new realtime_tools::RealtimePublisher <robot_msgs::Transform> (topic_ + "/transform", 1) ;
-  
+
   if (internal_state_publisher_ != NULL)// Make sure that we don't memory leak if initXml gets called twice
     delete internal_state_publisher_ ;
   internal_state_publisher_ = new realtime_tools::RealtimePublisher <robot_mechanism_controllers::PlugInternalState> (topic_ + "/internal_state", 1) ;
@@ -413,7 +414,7 @@
     if (current_frame_publisher_->trylock())
     {
       tf::Transform transform;
-      mechanism::TransformKDLToTF(controller_.endeffector_frame_, transform);
+      tf::TransformKDLToTF(controller_.endeffector_frame_, transform);
       tf::TransformTFToMsg(transform, current_frame_publisher_->msg_);
       current_frame_publisher_->unlockAndPublish() ;
      }
@@ -457,7 +458,7 @@
   controller_.outlet_norm_(1) = norm.y();
   controller_.outlet_norm_(2) = norm.z();
 
-  mechanism::TransformTFToKDL(outlet, controller_.desired_frame_);
+  tf::TransformTFToKDL(outlet, controller_.desired_frame_);
 }
 
 void PlugControllerNode::command()

Modified: pkg/trunk/mechanism/mechanism_model/include/mechanism_model/chain.h
===================================================================
--- pkg/trunk/mechanism/mechanism_model/include/mechanism_model/chain.h	2009-04-17 00:15:05 UTC (rev 13984)
+++ pkg/trunk/mechanism/mechanism_model/include/mechanism_model/chain.h	2009-04-17 00:15:33 UTC (rev 13985)
@@ -40,11 +40,6 @@
 
 namespace mechanism {
 
-void TransformTFToKDL(const tf::Transform &t, KDL::Frame &k);
-void TransformKDLToTF(const KDL::Frame &k, tf::Transform &t);
-void RotationTFToKDL(const tf::Quaternion& t, KDL::Rotation& k);
-void VectorTFToKDL(const tf::Vector3& t, KDL::Vector& k);
-
 class Chain
 {
 public:

Modified: pkg/trunk/mechanism/mechanism_model/manifest.xml
===================================================================
--- pkg/trunk/mechanism/mechanism_model/manifest.xml	2009-04-17 00:15:05 UTC (rev 13984)
+++ pkg/trunk/mechanism/mechanism_model/manifest.xml	2009-04-17 00:15:33 UTC (rev 13985)
@@ -9,6 +9,7 @@
 <depend package="tinyxml" />
 <depend package="wg_robot_description_parser" />
 <depend package="tf" />
+<depend package="tf_conversions" />
 <depend package="std_srvs" />
 <depend package="control_toolbox" />
 <depend package="bullet" />

Modified: pkg/trunk/mechanism/mechanism_model/src/chain.cpp
===================================================================
--- pkg/trunk/mechanism/mechanism_model/src/chain.cpp	2009-04-17 00:15:05 UTC (rev 13984)
+++ pkg/trunk/mechanism/mechanism_model/src/chain.cpp	2009-04-17 00:15:33 UTC (rev 13985)
@@ -30,37 +30,10 @@
 // Author: Stuart Glaser
 
 #include "mechanism_model/chain.h"
+#include "tf_conversions/tf_kdl.h"
 
-
 namespace mechanism {
 
-void TransformTFToKDL(const tf::Transform &t, KDL::Frame &k)
-{
-  for (unsigned int i = 0; i < 3; ++i)
-    k.p.data[i] = t.getOrigin()[i];
-  for (unsigned int i = 0; i < 9; ++i)
-    k.M.data[i] = t.getBasis()[i/3][i%3];
-}
-
-void TransformKDLToTF(const KDL::Frame &k, tf::Transform &t)
-{
-  t.setOrigin(tf::Vector3(k.p.data[0], k.p.data[1], k.p.data[2]));
-  t.setBasis(btMatrix3x3(k.M.data[0], k.M.data[1], k.M.data[2],
-                         k.M.data[3], k.M.data[4], k.M.data[5],
-                         k.M.data[6], k.M.data[7], k.M.data[8]));
-}
-
-void RotationTFToKDL(const tf::Quaternion& t, KDL::Rotation& k) 
-{
-  k.Quaternion(t[0], t[1], t[2], t[3]);
-}
-
-void VectorTFToKDL(const tf::Vector3& t, KDL::Vector& k) 
-{
-  k = KDL::Vector(t[0], t[1], t[2]);
-}
-
-
 bool Chain::init(Robot *robot, const std::string &root, const std::string &tip)
 {
   robot_ = robot;
@@ -163,32 +136,32 @@
   for (i = 0; i < link_indices_.size(); ++i)
   {
     // Here we create a KDL Segment for each part of the chain, between root and tip.
-    // A KDL Segment consists of a link and a joint. The joint is positioned at the 
+    // A KDL Segment consists of a link and a joint. The joint is positioned at the
     // beginning of the link, and connects this link to the previous link.
     KDL::Frame kdl_link;
     KDL::Vector pos;
     KDL::Rotation rot;
     KDL::Vector axis;
 
-    // Creates the link: a link is defined by a position and rotation, 
+    // Creates the link: a link is defined by a position and rotation,
     // relative to the reference frame of the previous link.
     if (i == 0){
       kdl_link = KDL::Frame::Identity();
     }
     // moving from root to ancestor
     else if (i<= reversed_index_){
-      VectorTFToKDL(robot_->links_[link_indices_[i-1]]->getOffset().getOrigin(), pos);
-      RotationTFToKDL(robot_->links_[link_indices_[i-1]]->getRotation().getRotation(), rot);
+      tf::VectorTFToKDL(robot_->links_[link_indices_[i-1]]->getOffset().getOrigin(), pos);
+      tf::RotationTFToKDL(robot_->links_[link_indices_[i-1]]->getRotation().getRotation(), rot);
       kdl_link = KDL::Frame(rot, pos).Inverse();
     }
     // moving from ancestor to tip
     else{
-      VectorTFToKDL(robot_->links_[link_indices_[i]]->getOffset().getOrigin(), pos);
-      RotationTFToKDL(robot_->links_[link_indices_[i]]->getRotation().getRotation(), rot);
+      tf::VectorTFToKDL(robot_->links_[link_indices_[i]]->getOffset().getOrigin(), pos);
+      tf::RotationTFToKDL(robot_->links_[link_indices_[i]]->getRotation().getRotation(), rot);
       kdl_link = KDL::Frame(rot, pos);
     }
 
-    // Creates the joint: a joint is defined by a position and an axis, 
+    // Creates the joint: a joint is defined by a position and an axis,
     // relative to the reference frame of the previous link.
     KDL::Joint kdl_joint;
     if (i == 0 || robot_->joints_[all_joint_indices_[i-1]]->type_ == JOINT_FIXED){
@@ -196,12 +169,12 @@
     }
     // moving from ancestor to tip
     else if (i<= reversed_index_){
-      VectorTFToKDL(robot_->joints_[all_joint_indices_[i-1]]->axis_, axis);
+      tf::VectorTFToKDL(robot_->joints_[all_joint_indices_[i-1]]->axis_, axis);
       kdl_joint = KDL::Joint(KDL::Vector::Zero(), kdl_link.M * axis * -1, KDL::Joint::RotAxis);
     }
     // moving from ancestor to tip
     else{
-      VectorTFToKDL(robot_->joints_[all_joint_indices_[i-1]]->axis_, axis);
+      tf::VectorTFToKDL(robot_->joints_[all_joint_indices_[i-1]]->axis_, axis);
       kdl_joint = KDL::Joint(kdl_link.p, axis, KDL::Joint::RotAxis);
   }
 


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