|
From: <stu...@us...> - 2009-04-17 00:15:40
|
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.
|