Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[18799] pkg/trunk
|
From: <mee...@us...> - 2009-07-14 22:24:14
|
Revision: 18799
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=18799&view=rev
Author: meeussen
Date: 2009-07-14 22:24:04 +0000 (Tue, 14 Jul 2009)
Log Message:
-----------
get the link name from the kdl segment
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/chain.h
pkg/trunk/stacks/mechanism/mechanism_model/src/chain.cpp
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-07-14 22:23:11 UTC (rev 18798)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/plug_controller.h 2009-07-14 22:24:04 UTC (rev 18799)
@@ -89,6 +89,7 @@
KDL::Frame desired_frame_;
mechanism::Chain chain_;
+ KDL::Chain kdl_chain_;
double dist_to_line_;
double f_r_;
@@ -101,7 +102,6 @@
mechanism::RobotState *robot_;
std::string controller_name_;
- KDL::Chain kdl_chain_;
boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_;
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-07-14 22:23:11 UTC (rev 18798)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-07-14 22:24:04 UTC (rev 18799)
@@ -467,13 +467,13 @@
ros::Node *node = ros::Node::instance();
- task_frame_name_ = c_.chain_.getLinkName(0);
+ task_frame_name_ = c_.kdl_chain_.getSegment(0).getName();
//node->subscribe(name_ + "/command", command_msg_, &CartesianHybridControllerNode::command, this, 5);
command_notifier_.reset(new tf::MessageNotifier<manipulation_msgs::TaskFrameFormalism>(
&TF, node,
boost::bind(&CartesianHybridControllerNode::command, this, _1),
- name_ + "/command", c_.chain_.getLinkName(0), 100));
+ name_ + "/command", c_.kdl_chain_.getSegment(0).getName(), 100));
pub_state_.reset(new realtime_tools::RealtimePublisher<robot_mechanism_controllers::CartesianHybridState>(name_ + "/state", 1));
pub_tf_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>("/tf_message", 5));
@@ -542,7 +542,7 @@
{
//pub_tf_->msg_.transforms[0].header.stamp.fromSec();
pub_tf_->msg_.transforms[0].header.frame_id = name_ + "/tool_frame";
- pub_tf_->msg_.transforms[0].parent_id = c_.chain_.getLinkName();
+ pub_tf_->msg_.transforms[0].parent_id = c_.kdl_chain_.getSegment(c_.kdl_chain_.getNrOfSegments()-1).getName();
tf::Transform t;
tf::TransformKDLToTF(c_.tool_frame_offset_, t);
tf::transformTFToMsg(t, pub_tf_->msg_.transforms[0].transform);
@@ -558,7 +558,7 @@
tf::Stamped<tf::Transform> task_frame;
try {
- TF.lookupTransform(c_.chain_.getLinkName(0), tff_msg->header.frame_id, tff_msg->header.stamp,
+ TF.lookupTransform(c_.kdl_chain_.getSegment(0).getName(), tff_msg->header.frame_id, tff_msg->header.stamp,
task_frame);
}
catch (tf::TransformException &ex)
@@ -597,17 +597,17 @@
robot_srvs::SetPoseStamped::Request &req,
robot_srvs::SetPoseStamped::Response &resp)
{
- if (!TF.canTransform(c_.chain_.getLinkName(-1), req.p.header.frame_id,
+ if (!TF.canTransform(c_.kdl_chain_.getSegment(c_.kdl_chain_.getNrOfSegments()-1).getName(), req.p.header.frame_id,
req.p.header.stamp, ros::Duration(3.0)))
{
- ROS_ERROR("Cannot transform %s -> %s at %lf", c_.chain_.getLinkName(-1).c_str(),
+ ROS_ERROR("Cannot transform %s -> %s at %lf", c_.kdl_chain_.getSegment(c_.kdl_chain_.getNrOfSegments()-1).getName().c_str(),
req.p.header.frame_id.c_str(), req.p.header.stamp.toSec());
return false;
}
robot_msgs::PoseStamped tool_in_tip_msg;
tf::Transform tool_in_tip;
- TF.transformPose(c_.chain_.getLinkName(-1), req.p, tool_in_tip_msg);
+ TF.transformPose(c_.kdl_chain_.getSegment(c_.kdl_chain_.getNrOfSegments()-1).getName(), req.p, tool_in_tip_msg);
tf::poseMsgToTF(tool_in_tip_msg.pose, tool_in_tip);
tool_in_tip.setOrigin(tf::Vector3(0,0,0));
tf::TransformTFToKDL(tool_in_tip, c_.tool_frame_offset_);
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-07-14 22:23:11 UTC (rev 18798)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/plug_controller.cpp 2009-07-14 22:24:04 UTC (rev 18799)
@@ -481,7 +481,8 @@
robot_msgs::PoseStamped tool_offset_msg;
try
{
- TF.transformPose(controller_.chain_.getLinkName(), req.p, tool_offset_msg);
+ //TF.transformPose(controller_.chain_.getLinkName(), req.p, tool_offset_msg);
+ TF.transformPose(controller_.kdl_chain_.getSegment(controller_.kdl_chain_.getNrOfSegments()-1).getName(), req.p, tool_offset_msg);
}
catch(tf::TransformException& ex)
{
Modified: pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/chain.h
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/chain.h 2009-07-14 22:23:11 UTC (rev 18798)
+++ pkg/trunk/stacks/mechanism/mechanism_model/include/mechanism_model/chain.h 2009-07-14 22:24:04 UTC (rev 18799)
@@ -66,7 +66,6 @@
// get KDL chain
void toKDL(KDL::Chain &chain);
- std::string getLinkName(int index = -1);
Joint* getJoint(unsigned int actuated_joint_i);
private:
@@ -75,7 +74,6 @@
std::vector<int> joint_indices_; // ONLY joints that can be actuated (not fixed joints)
std::vector<int> all_joint_indices_; // Includes fixed joints
- std::vector<std::string> link_names_;
};
}
Modified: pkg/trunk/stacks/mechanism/mechanism_model/src/chain.cpp
===================================================================
--- pkg/trunk/stacks/mechanism/mechanism_model/src/chain.cpp 2009-07-14 22:23:11 UTC (rev 18798)
+++ pkg/trunk/stacks/mechanism/mechanism_model/src/chain.cpp 2009-07-14 22:24:04 UTC (rev 18799)
@@ -149,14 +149,5 @@
}
-std::string Chain::getLinkName(int index)
-{
- if (index == -1)
- index = link_names_.size() - 1;
- return link_names_[index];
}
-
-
-
-}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
