Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[11821] pkg/trunk
|
From: <ge...@us...> - 2009-02-26 18:21:10
|
Revision: 11821
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=11821&view=rev
Author: gerkey
Date: 2009-02-26 18:21:08 +0000 (Thu, 26 Feb 2009)
Log Message:
-----------
Added header to robot_srvs/IKService, to allow EE pose to be given in
non-torso_link frame. Added logic to pr2_ik_node to try to transform from
given frame to torso_link for a given timeout. Modified "test" program to
match.
Modified Paths:
--------------
pkg/trunk/prcore/robot_srvs/srv/IKService.srv
pkg/trunk/util/kinematics/libKinematics/include/libKinematics/pr2_ik_node.h
pkg/trunk/util/kinematics/libKinematics/src/pr2_ik_node.cpp
pkg/trunk/util/kinematics/libKinematics/test/test_pr2_ik_node.cpp
Modified: pkg/trunk/prcore/robot_srvs/srv/IKService.srv
===================================================================
--- pkg/trunk/prcore/robot_srvs/srv/IKService.srv 2009-02-26 18:12:48 UTC (rev 11820)
+++ pkg/trunk/prcore/robot_srvs/srv/IKService.srv 2009-02-26 18:21:08 UTC (rev 11821)
@@ -1,4 +1,6 @@
+Header header
robot_msgs/Pose pose
robot_msgs/JointTrajPoint joint_pos
+duration timeout
---
robot_msgs/JointTraj traj
Modified: pkg/trunk/util/kinematics/libKinematics/include/libKinematics/pr2_ik_node.h
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/include/libKinematics/pr2_ik_node.h 2009-02-26 18:12:48 UTC (rev 11820)
+++ pkg/trunk/util/kinematics/libKinematics/include/libKinematics/pr2_ik_node.h 2009-02-26 18:21:08 UTC (rev 11821)
@@ -39,6 +39,7 @@
#include <robot_srvs/IKService.h>
#include <tf/tf.h>
+#include <tf/transform_listener.h>
#include <LinearMath/btTransform.h>
using namespace kinematics;
@@ -66,6 +67,12 @@
bool init();
double init_solution_theta3_;
+
+ // Transform incoming EE pose to the frame that we're hardwired for,
+ // obeying timeout in the request. Return true on success, false
+ // otherwise.
+ bool transformPose(robot_srvs::IKService::Request& req,
+ tf::Stamped<tf::Pose>& pose_out);
bool processIKRequest(robot_srvs::IKService::Request &req, robot_srvs::IKService::Response &resp);
@@ -73,6 +80,8 @@
private:
+ static std::string getTargetFrame() { return std::string("torso_link"); }
+
int closestJointSolution(const std::vector<double> current_joint_pos, const std::vector<std::vector<double> > new_positions);
std::string robot_description_model_;
@@ -95,5 +104,7 @@
double root_z_;
+ tf::TransformListener tf_;
+
};
}
Modified: pkg/trunk/util/kinematics/libKinematics/src/pr2_ik_node.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/src/pr2_ik_node.cpp 2009-02-26 18:12:48 UTC (rev 11820)
+++ pkg/trunk/util/kinematics/libKinematics/src/pr2_ik_node.cpp 2009-02-26 18:21:08 UTC (rev 11821)
@@ -34,7 +34,7 @@
#include <libKinematics/pr2_ik_node.h>
#include <angles/angles.h>
-LibKinematicsNode::LibKinematicsNode(std::string node_name,std::string arm_name):ros::Node(node_name),arm_name_(arm_name),increment_(0.01),root_x_(0.0),root_y_(0.0), root_z_(0.0)
+LibKinematicsNode::LibKinematicsNode(std::string node_name,std::string arm_name):ros::Node(node_name),arm_name_(arm_name),increment_(0.01),root_x_(0.0),root_y_(0.0), root_z_(0.0), tf_(*this)
{
advertiseService("perform_pr2_ik", &LibKinematicsNode::processIKRequest);
advertiseService("perform_pr2_ik_closest", &LibKinematicsNode::processIKClosestRequest);
@@ -189,13 +189,54 @@
return true;
}
+// Transform incoming EE pose to the frame that we're hardwired for,
+// obeying timeout in the request. Return true on success, false
+// otherwise.
+bool LibKinematicsNode::transformPose(robot_srvs::IKService::Request& req,
+ tf::Stamped<tf::Pose>& pose_out)
+{
+ tf::Stamped<tf::Pose> tf_pose_in;
+ tf::PoseMsgToTF(req.pose, tf_pose_in);
+
+ tf_pose_in.stamp_ = req.header.stamp;
+ tf_pose_in.frame_id_ = req.header.frame_id;
+
+ // Transform into the desired frame
+ ros::Time start = ros::Time::now();
+ bool transformed = false;
+ // Heuristically chosen value
+ ros::Duration sleeptime = ros::Duration().fromSec(0.01);
+ while((req.timeout.toSec() == 0.0) ||
+ ((ros::Time::now() - start) < req.timeout))
+ {
+ try
+ {
+ tf_.transformPose(getTargetFrame(), tf_pose_in, pose_out);
+ transformed = true;
+ break;
+ }
+ catch(tf::TransformException e)
+ {
+ // Try, try, again
+ sleeptime.sleep();
+ }
+ }
+
+ return transformed;
+}
+
bool LibKinematicsNode::processIKRequest(robot_srvs::IKService::Request &req, robot_srvs::IKService::Response &resp)
{
- robot_msgs::Pose pose;
- pose = req.pose;
+ tf::Stamped<tf::Pose> tf_pose;
- tf::Pose tf_pose;
- tf::PoseMsgToTF(pose,tf_pose);
+ if(!transformPose(req, tf_pose))
+ {
+ ROS_WARN("Failed to transform from %s to %s within timeout (%f)",
+ req.header.frame_id.c_str(),
+ getTargetFrame().c_str(),
+ req.timeout.toSec());
+ return false;
+ }
NEWMAT::Matrix g0(4,4);
btScalar m[16];
@@ -236,11 +277,17 @@
bool LibKinematicsNode::processIKClosestRequest(robot_srvs::IKService::Request &req, robot_srvs::IKService::Response &resp)
{
- robot_msgs::Pose pose;
- pose = req.pose;
+ tf::Stamped<tf::Pose> tf_pose;
- tf::Pose tf_pose;
- tf::PoseMsgToTF(pose,tf_pose);
+ if(!transformPose(req, tf_pose))
+ {
+ ROS_WARN("Failed to transform from %s to %s within timeout (%f)",
+ req.header.frame_id.c_str(),
+ getTargetFrame().c_str(),
+ req.timeout.toSec());
+ return false;
+ }
+
int num_joints = req.joint_pos.positions.size();
if(num_joints != 7)
Modified: pkg/trunk/util/kinematics/libKinematics/test/test_pr2_ik_node.cpp
===================================================================
--- pkg/trunk/util/kinematics/libKinematics/test/test_pr2_ik_node.cpp 2009-02-26 18:12:48 UTC (rev 11820)
+++ pkg/trunk/util/kinematics/libKinematics/test/test_pr2_ik_node.cpp 2009-02-26 18:21:08 UTC (rev 11821)
@@ -58,6 +58,12 @@
robot_srvs::IKService::Request req;
robot_srvs::IKService::Response res;
+ req.header.frame_id = "torso_link";
+ // Timeout is how long we're willing to wait for the transform from
+ // our frame to the ik_node's hardcoded working frame. 0.0 is forever
+ // (dangerous!).
+ req.timeout.fromSec(0.1);
+
tf::Pose pose(btQuaternion(0,0,0),btVector3(0.75,-0.288,0));
tf::PoseTFToMsg(pose,req.pose);
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
