Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[12564] pkg/trunk
|
From: <stu...@us...> - 2009-03-17 00:56:38
|
Revision: 12564
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12564&view=rev
Author: stuglaser
Date: 2009-03-17 00:56:35 +0000 (Tue, 17 Mar 2009)
Log Message:
-----------
Dump of changes made for plugging in.
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
pkg/trunk/demos/plug_in/fake_vision.py
pkg/trunk/demos/plug_in/manifest.xml
Added Paths:
-----------
pkg/trunk/demos/plug_in/CMakeLists.txt
pkg/trunk/demos/plug_in/Makefile
pkg/trunk/demos/plug_in/outlet_pose_filter.py
pkg/trunk/demos/plug_in/plug_in2.py
pkg/trunk/demos/plug_in/src/
pkg/trunk/demos/plug_in/src/servo.cpp
pkg/trunk/prcore/robot_msgs/msg/CartesianState.msg
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h 2009-03-17 00:54:11 UTC (rev 12563)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/cartesian_hybrid_controller.h 2009-03-17 00:56:35 UTC (rev 12564)
@@ -56,6 +56,7 @@
virtual void update(void);
virtual bool starting();
+ KDL::Twist pose_desi_;
KDL::Twist twist_desi_;
KDL::Wrench wrench_desi_;
KDL::Frame pose_meas_;
@@ -75,6 +76,8 @@
KDL::Chain kdl_chain_;
mechanism::RobotState *robot_;
double last_time_;
+
+ int initial_mode_;
};
class CartesianHybridControllerNode : public Controller
@@ -100,8 +103,11 @@
ros::Node *node_;
robot_msgs::TaskFrameFormalism command_msg_;
+ std::string task_frame_name_;
+
unsigned int loop_count_;
boost::scoped_ptr<realtime_tools::RealtimePublisher<robot_msgs::CartesianState> > pub_state_;
+ boost::scoped_ptr<realtime_tools::RealtimePublisher<tf::tfMessage> > pub_tf_;
};
}
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-03-17 00:54:11 UTC (rev 12563)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/cartesian_hybrid_controller.cpp 2009-03-17 00:56:35 UTC (rev 12564)
@@ -40,6 +40,7 @@
#include "realtime_tools/realtime_publisher.h"
#include "control_toolbox/pid.h"
#include "robot_msgs/VisualizationMarker.h"
+#include "angles/angles.h"
namespace controller {
@@ -122,6 +123,9 @@
for (size_t i = 3; i < 6; ++i)
twist_pids_[i] = temp_pid;
+ if (!node->getParam(name + "/initial_mode", initial_mode_))
+ initial_mode_ = robot_msgs::TaskFrameFormalism::FORCE;
+
// Default commands
task_frame_offset_ = KDL::Frame::Identity();
@@ -162,15 +166,18 @@
{
twist_desi_[i] = 0;
wrench_desi_[i] = 0;
+ pose_desi_[i] = 0;
double setpoint = setpoint_[i];
switch(mode_[i])
{
case robot_msgs::TaskFrameFormalism::POSITION:
+ pose_desi_[i] = setpoint;
if (i < 3) // Translational position
setpoint = pose_pids_[i].updatePid(tool.p.p[i] - setpoint, dt);
else // Rotational position
- setpoint = pose_pids_[i].updatePid(rpy[i - 3] - setpoint, dt);
+ //setpoint = pose_pids_[i].updatePid(angles::shortest_angular_distance(rpy[i - 3], setpoint), dt);
+ setpoint = pose_pids_[i].updatePid(angles::shortest_angular_distance(setpoint, rpy[i - 3]), dt);
case robot_msgs::TaskFrameFormalism::VELOCITY:
twist_desi_[i] = setpoint;
setpoint = twist_pids_[i].updatePid(tool.GetTwist()[i] - setpoint, dt);
@@ -208,11 +215,45 @@
bool CartesianHybridController::starting()
{
- for (size_t i = 0; i < 6; ++i)
+ task_frame_offset_ = KDL::Frame::Identity();
+ tool_frame_offset_ = KDL::Frame::Identity();
+
+
+ switch(initial_mode_)
{
- mode_[i] = robot_msgs::TaskFrameFormalism::FORCE;
- setpoint_[i] = 0.0;
+ case robot_msgs::TaskFrameFormalism::POSITION: {
+ // Finds the starting pose/twist
+ KDL::JntArrayVel jnt_vel(kdl_chain_.getNrOfJoints());
+ chain_.getVelocities(robot_->joint_states_, jnt_vel);
+ KDL::FrameVel frame;
+ KDL::ChainFkSolverVel_recursive fkvel_solver(kdl_chain_);
+ fkvel_solver.JntToCart(jnt_vel, frame);
+
+ for (size_t i = 0; i < 6; ++i) {
+ mode_[i] = initial_mode_;
+ }
+ for (size_t i = 0; i < 3; ++i) {
+ setpoint_[i] = frame.p.p[i];
+ }
+ frame.M.R.GetRPY(setpoint_[3], setpoint_[4], setpoint_[5]);
+ break;
}
+ case robot_msgs::TaskFrameFormalism::VELOCITY:
+ for (size_t i = 0; i < 6; ++i) {
+ mode_[i] = initial_mode_;
+ setpoint_[i] = 0.0;
+ }
+ break;
+ case robot_msgs::TaskFrameFormalism::FORCE:
+ for (size_t i = 0; i < 6; ++i) {
+ mode_[i] = initial_mode_;
+ setpoint_[i] = 0.0;
+ }
+ break;
+ default:
+ return false;
+ }
+
return true;
}
@@ -241,10 +282,14 @@
ros::Node *node = ros::Node::instance();
- node->subscribe(name_ + "/command", command_msg_, &CartesianHybridControllerNode::command, this, 1);
+ task_frame_name_ = c_.chain_.getLinkName(0);
+
+ node->subscribe(name_ + "/command", command_msg_, &CartesianHybridControllerNode::command, this, 5);
node->advertiseService(name_ + "/set_tool_frame", &CartesianHybridControllerNode::setToolFrame, this);
pub_state_.reset(new realtime_tools::RealtimePublisher<robot_msgs::CartesianState>(name_ + "/state", 1));
+ pub_tf_.reset(new realtime_tools::RealtimePublisher<tf::tfMessage>("/tf_message", 5));
+ pub_tf_->msg_.transforms.resize(1);
return true;
@@ -252,6 +297,8 @@
void CartesianHybridControllerNode::update()
{
+
+ KDL::Twist last_pose_desi = c_.pose_desi_;
KDL::Twist last_twist_desi = c_.twist_desi_;
KDL::Wrench last_wrench_desi = c_.wrench_desi_;
@@ -262,13 +309,26 @@
if (pub_state_->trylock())
{
pub_state_->msg_.header.frame_id = "TODO___THE_TASK_FRAME";
- TransformKDLToMsg(c_.pose_meas_, pub_state_->msg_.last_pose_meas);
+ TransformKDLToMsg(c_.pose_meas_, pub_state_->msg_.last_pose_meas.pose);
+ pub_state_->msg_.last_pose_meas.header.frame_id = task_frame_name_;
+ pub_state_->msg_.last_pose_meas.header.stamp = ros::Time(c_.last_time_);
+ TwistKDLToMsg(last_pose_desi, pub_state_->msg_.last_pose_desi);
TwistKDLToMsg(c_.twist_meas_, pub_state_->msg_.last_twist_meas);
TwistKDLToMsg(last_twist_desi, pub_state_->msg_.last_twist_desi);
WrenchKDLToMsg(last_wrench_desi, pub_state_->msg_.last_wrench_desi);
pub_state_->unlockAndPublish();
}
+ if (pub_tf_->trylock())
+ {
+ //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();
+ tf::Transform t;
+ mechanism::TransformKDLToTF(c_.tool_frame_offset_, t);
+ tf::TransformTFToMsg(t, pub_tf_->msg_.transforms[0].transform);
+ pub_tf_->unlockAndPublish();
+ }
}
}
@@ -318,8 +378,16 @@
void CartesianHybridControllerNode::command()
{
+ task_frame_name_ = command_msg_.header.frame_id;
tf::Stamped<tf::Transform> task_frame;
+
try {
+ ROS_INFO("Waiting on transform (%.3lf vs %.3lf)", command_msg_.header.stamp.toSec(), ros::Time::now().toSec());
+ while (!TF.canTransform(c_.chain_.getLinkName(0), command_msg_.header.frame_id, command_msg_.header.stamp))
+ {
+ usleep(10000);
+ }
+ ROS_INFO("Got transform.");
TF.lookupTransform(c_.chain_.getLinkName(0), command_msg_.header.frame_id, command_msg_.header.stamp,
task_frame);
}
Added: pkg/trunk/demos/plug_in/CMakeLists.txt
===================================================================
--- pkg/trunk/demos/plug_in/CMakeLists.txt (rev 0)
+++ pkg/trunk/demos/plug_in/CMakeLists.txt 2009-03-17 00:56:35 UTC (rev 12564)
@@ -0,0 +1,6 @@
+cmake_minimum_required(VERSION 2.4.6)
+include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
+rospack(plug_in)
+
+#rospack_add_boost_directories()
+rospack_add_executable(servo src/servo.cpp)
Added: pkg/trunk/demos/plug_in/Makefile
===================================================================
--- pkg/trunk/demos/plug_in/Makefile (rev 0)
+++ pkg/trunk/demos/plug_in/Makefile 2009-03-17 00:56:35 UTC (rev 12564)
@@ -0,0 +1 @@
+include $(shell rospack find mk)/cmake.mk
Modified: pkg/trunk/demos/plug_in/fake_vision.py
===================================================================
--- pkg/trunk/demos/plug_in/fake_vision.py 2009-03-17 00:54:11 UTC (rev 12563)
+++ pkg/trunk/demos/plug_in/fake_vision.py 2009-03-17 00:56:35 UTC (rev 12564)
@@ -27,9 +27,7 @@
# POSSIBILITY OF SUCH DAMAGE.
import roslib
-roslib.load_manifest('pr2_mechanism_controllers')
-roslib.load_manifest('std_msgs')
-roslib.load_manifest('rospy')
+roslib.load_manifest('plug_in')
import rospy
from std_msgs.msg import *
@@ -37,24 +35,17 @@
from tf.msg import tfMessage
from math import *
from time import sleep
+import tf.transformations
+
def xyz(x, y, z):
p = Point()
p.x, p.y, p.z = x, y, z
return p
-def rpy(y, p, r):
- q = Quaternion()
- cosY = cos(y / 2.)
- sinY = sin(y / 2.)
- cosP = cos(p / 2.)
- sinP = sin(p / 2.)
- cosR = cos(r / 2.)
- sinR = sin(r / 2.)
- q.x = cosR*cosP*cosY + sinR*sinP*sinY
- q.y = sinR*cosP*cosY + cosR*sinP*sinY
- q.z = cosR*sinP*cosY + sinR*cosP*sinY
- q.w = cosR*cosP*sinY + sinR*sinP*cosY
+def rpy(r, p, y):
+ a = tf.transformations.quaternion_from_euler(r, p, y, "rzyx")
+ q = Quaternion(a[0], a[1], a[2], a[3])
return q
@@ -68,6 +59,7 @@
mechanism_state = Tracker('/mechanism_state', MechanismState)
def last_time():
+ return rospy.rostime.get_rostime()
global mechanism_state
if mechanism_state.msg:
return mechanism_state.msg.header.stamp
@@ -75,23 +67,23 @@
-pub_outlet = rospy.Publisher('/outlet_pose', PoseStamped)
-pub_plug = rospy.Publisher('/plug_pose', PoseStamped)
+pub_outlet = rospy.Publisher('/outlet_detector/pose', PoseStamped)
+pub_plug = rospy.Publisher('/plug_detector/pose', PoseStamped)
rospy.init_node('fake_vision', anonymous=True)
-while last_time() == 0:
- pass
+sleep(0.2)
+
def send():
op = PoseStamped()
op.header.stamp = last_time()
op.header.frame_id = 'torso_lift_link'
- op.pose.position = xyz(0.7, -0.4, -0.2)
+ op.pose.position = xyz(0.7, -0.4, -0.4)
op.pose.orientation = rpy(0, 0, 0)
pp = PoseStamped()
pp.header.stamp = last_time()
pp.header.frame_id = 'r_gripper_tool_frame'
pp.pose.position = xyz(0.0, 0.0, 0.0)
- pp.pose.orientation = rpy(0,pi/6,0)
+ pp.pose.orientation = rpy(0,-pi/6,0)
print "Publishing..."
for i in range(10):
Modified: pkg/trunk/demos/plug_in/manifest.xml
===================================================================
--- pkg/trunk/demos/plug_in/manifest.xml 2009-03-17 00:54:11 UTC (rev 12563)
+++ pkg/trunk/demos/plug_in/manifest.xml 2009-03-17 00:56:35 UTC (rev 12564)
@@ -9,4 +9,5 @@
<depend package="pr2_etherCAT" />
<depend package="joy" />
<depend package="outlet_detection" />
+ <depend package="tf" />
</package>
Added: pkg/trunk/demos/plug_in/outlet_pose_filter.py
===================================================================
--- pkg/trunk/demos/plug_in/outlet_pose_filter.py (rev 0)
+++ pkg/trunk/demos/plug_in/outlet_pose_filter.py 2009-03-17 00:56:35 UTC (rev 12564)
@@ -0,0 +1,87 @@
+#! /usr/bin/env python
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in the
+# documentation and/or other materials provided with the distribution.
+# * Neither the name of the Willow Garage, Inc. nor the names of its
+# contributors may be used to endorse or promote products derived from
+# this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+import sys, time
+import roslib; roslib.load_manifest('plug_in')
+import rospy
+from robot_msgs.msg import PoseStamped, TransformStamped
+import tf.msg
+from std_srvs.srv import Empty
+
+class Tracker:
+ def __init__(self, topic, Msg):
+ self.sub = rospy.Subscriber(topic, Msg, self.callback)
+ self.msg = None
+
+ def callback(self, msg):
+ self.msg = msg
+
+def pose_to_transform_stamped(p):
+ t = TransformStamped()
+ t.header.stamp = p.header.stamp
+ #t.header.frame_id = p.header.frame_id
+ t.parent_id = p.header.frame_id
+ t.transform.translation.x = p.pose.position.x
+ t.transform.translation.y = p.pose.position.y
+ t.transform.translation.z = p.pose.position.z
+ t.transform.rotation = p.pose.orientation
+ return t
+
+def main():
+
+ # TODO: outlet pose remains stable in the base frame. Transform
+ # to there, and then continuously publish
+
+ track_outlet_pose = Tracker('/outlet_detector/pose', PoseStamped)
+
+ def reset():
+ track_outlet_pose.msg = None
+ rospy.Service('/outlet_pose_filter/reset', Empty, reset)
+
+ tf_pub = rospy.Publisher('/tf_message', tf.msg.tfMessage)
+ tf_msg = tf.msg.tfMessage()
+ tf_msg.transforms = [TransformStamped()]
+
+ rospy.init_node('outlet_filter')
+
+ seq = 0
+ outlet_msg = None
+ while not rospy.is_shutdown():
+ if outlet_msg: # and outlet_msg.pose.orientation.z > 0: # HACK
+ tf_msg.transforms = [pose_to_transform_stamped(outlet_msg)]
+ tf_msg.transforms[0].header.seq = seq
+ tf_msg.transforms[0].header.frame_id = 'outlet_pose'
+ tf_msg.transforms[0].header.stamp = rospy.rostime.get_rostime()
+ tf_pub.publish(tf_msg)
+ seq += 1
+ else:
+ outlet_msg = track_outlet_pose.msg
+
+ time.sleep(0.01)
+
+if __name__ == '__main__': main()
Property changes on: pkg/trunk/demos/plug_in/outlet_pose_filter.py
___________________________________________________________________
Added: svn:executable
+ *
Added: pkg/trunk/demos/plug_in/plug_in2.py
===================================================================
--- pkg/trunk/demos/plug_in/plug_in2.py (rev 0)
+++ pkg/trunk/demos/plug_in/plug_in2.py 2009-03-17 00:56:35 UTC (rev 12564)
@@ -0,0 +1,274 @@
+#! /usr/bin/env python
+# Copyright (c) 2008, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright
+# notice, this list of conditions and the following disclaimer in the
+# documentation and/or other materials provided with the distribution.
+# * Neither the name of the Willow Garage, Inc. nor the names of its
+# contributors may be used to endorse or promote products derived from
+# this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# Author: Stuart Glaser
+
+import sys, time
+import math
+
+import roslib
+roslib.load_manifest('plug_in')
+import rospy
+from robot_msgs.msg import *
+from robot_srvs.srv import *
+import tf.transformations
+
+CONTROLLER = 'arm_constraint'
+
+class Tracker:
+ def __init__(self, topic, Msg):
+ self.sub = rospy.Subscriber(topic, Msg, self.callback)
+ self.msg = None
+
+ def callback(self, msg):
+ self.msg = msg
+
+
+
+mechanism_state = Tracker('/mechanism_state', MechanismState)
+def last_time():
+ global mechanism_state
+ if mechanism_state.msg:
+ return mechanism_state.msg.header.stamp
+ return 0
+
+def xyz(x, y, z):
+ p = Point()
+ p.x, p.y, p.z = x, y, z
+ return p
+
+def rpy(r, p, y):
+ a = tf.transformations.quaternion_from_euler(r, p, y, "rzyx")
+ q = Quaternion(a[0], a[1], a[2], a[3])
+ return q
+
+
+def main():
+ rospy.init_node('plug_in')
+
+ track_outlet_pose = Tracker('/outlet_detector/pose', PoseStamped)
+ track_plug_pose = Tracker('/plug_detector/pose', PoseStamped)
+
+ print "Waiting for mechanism control"
+ rospy.wait_for_service('kill_and_spawn_controllers')
+ kill_and_spawn = rospy.ServiceProxy('kill_and_spawn_controllers', KillAndSpawnControllers)
+ if rospy.is_shutdown(): sys.exit(0)
+
+ # Bring down all possible controllers, just in case
+ kill_and_spawn('<controllers></controllers>', ['arm_pose', CONTROLLER, 'arm_hybrid'])
+
+ ###### Finds the outlet
+
+ # Waits for an estimate of the outlet pose
+ print "Waiting for outlet pose..."
+ outlet_pose = None
+ msg = None
+ while not msg:
+ msg = track_outlet_pose.msg
+ if rospy.is_shutdown(): return
+ time.sleep(0.1)
+ outlet_pose = msg
+ print "Found outlet"
+
+ ###### Stages the plug
+
+ # Spawns the pose controller
+ print "Spawning the pose controller"
+ rospy.set_param("/arm_pose/p", 15.0)
+ rospy.set_param("/arm_pose/i", 0.2)
+ rospy.set_param("/arm_pose/d", 0.0)
+ rospy.set_param("/arm_pose/i_clamp", 0.5)
+ rospy.set_param("/arm_pose/root_name", "torso_lift_link")
+ rospy.set_param("/arm_pose/tip_name", "r_gripper_tool_frame")
+ rospy.set_param("/arm_pose/twist/fb_trans/p", 15.0)
+ rospy.set_param("/arm_pose/twist/fb_trans/i", 0.5)
+ rospy.set_param("/arm_pose/twist/fb_trans/d", 0.0)
+ rospy.set_param("/arm_pose/twist/fb_trans/i_clamp", 1.0)
+ rospy.set_param("/arm_pose/twist/fb_rot/p", 1.0)
+ rospy.set_param("/arm_pose/twist/fb_rot/i", 0.1)
+ rospy.set_param("/arm_pose/twist/fb_rot/d", 0.0)
+ rospy.set_param("/arm_pose/twist/fb_rot/i_clamp", 0.2)
+ pose_config = '<controller name="arm_pose" type="CartesianPoseControllerNode" />'
+ resp = kill_and_spawn(pose_config, [])
+ if len(resp.add_name) == 0 or not resp.add_ok[0]:
+ raise "Failed to spawn the pose controller"
+
+ # Commands the hand to near the outlet
+ print "Staging the plug"
+ staging_pose = PoseStamped()
+ staging_pose.header.frame_id = 'outlet_pose'
+ staging_pose.pose.position = xyz(-0.1, 0.0, 0.0)
+ staging_pose.pose.orientation = rpy(0,0,0)
+ pub_pose = rospy.Publisher('/arm_pose/command', PoseStamped)
+ for i in range(50):
+ staging_pose.header.stamp = last_time()
+ pub_pose.publish(staging_pose)
+ time.sleep(0.1)
+ time.sleep(1)
+
+ ###### Finds an initial estimate of the plug pose
+
+ # TODO: should wait for the hand to settle
+ print "Waiting for the first plug pose estimate"
+ track_plug_pose.msg = None
+ while not track_plug_pose.msg:
+ if rospy.is_shutdown(): return
+ time.sleep(0.1)
+ plug_pose = track_plug_pose.msg
+ print "Found plug"
+
+ if False: # Constraint controller
+
+ ###### Switches to the constraint controller
+
+ print "Spawning the constraint controller"
+ rospy.set_param("/arm_constraint/line_pid/p", 750.0)
+ rospy.set_param("/arm_constraint/line_pid/i", 1.0)
+ rospy.set_param("/arm_constraint/line_pid/d", 5.0)
+ rospy.set_param("/arm_constraint/line_pid/i_clamp", 10.0)
+ rospy.set_param("/arm_constraint/pose_pid/p", 35.0)
+ rospy.set_param("/arm_constraint/pose_pid/i", 8.0)
+ rospy.set_param("/arm_constraint/pose_pid/d", 1.0)
+ rospy.set_param("/arm_constraint/pose_pid/i_clamp", 10.0)
+ rospy.set_param("/arm_constraint/f_r_max", 150.0)
+ constraint_config = open('controllers.xml').read()
+ resp = kill_and_spawn(constraint_config, ['arm_pose'])
+ if len(resp.add_name) == 0 or not resp.add_ok[0]:
+ raise "Failed to spawn the constraint controller"
+
+ print "Setting tool frame"
+ rospy.wait_for_service("/%s/set_tool_frame" % CONTROLLER)
+ if rospy.is_shutdown(): return
+ set_tool_frame = rospy.ServiceProxy("/%s/set_tool_frame" % CONTROLLER, SetPoseStamped)
+ set_tool_frame(plug_pose)
+ print "Tool frame set"
+ time.sleep(1)
+
+
+ ###### Visual differencing loop over the plug pose estimate
+
+ pub_command = rospy.Publisher("/%s/outlet_pose" % CONTROLLER, PoseStamped)
+ cnt = 0
+ while not rospy.is_shutdown():
+ cnt += 1
+ outlet_pose.header.stamp = last_time()
+ pub_command.publish(outlet_pose)
+ if cnt % 3 == 0:
+ set_tool_frame(track_plug_pose.msg)
+ time.sleep(1.0)
+
+ ###### Attempt to plug in
+
+ else:
+
+ ###### Switches to the hybrid controller
+
+ print "Spawning the hybrid controller"
+ rospy.set_param("/arm_hybrid/type", "CartesianHybridControllerNode")
+ rospy.set_param("/arm_hybrid/initial_mode", 3)
+ rospy.set_param("/arm_hybrid/root_link", "torso_lift_link")
+ rospy.set_param("/arm_hybrid/tip_link", "r_gripper_tool_frame")
+ rospy.set_param("/arm_hybrid/fb_pose/p", 30.0)#20.0)
+ rospy.set_param("/arm_hybrid/fb_pose/i", 4.0)
+ rospy.set_param("/arm_hybrid/fb_pose/d", 0.0)
+ rospy.set_param("/arm_hybrid/fb_pose/i_clamp", 2.0)
+ rospy.set_param("/arm_hybrid/fb_trans_vel/p", 15.0)
+ rospy.set_param("/arm_hybrid/fb_trans_vel/i", 1.5)
+ rospy.set_param("/arm_hybrid/fb_trans_vel/d", 0.0)
+ rospy.set_param("/arm_hybrid/fb_trans_vel/i_clamp", 6.0)
+ rospy.set_param("/arm_hybrid/fb_rot_vel/p", 1.2)
+ rospy.set_param("/arm_hybrid/fb_rot_vel/i", 0.2)
+ rospy.set_param("/arm_hybrid/fb_rot_vel/d", 0.0)
+ rospy.set_param("/arm_hybrid/fb_rot_vel/i_clamp", 0.4)
+ hybrid_config = '<controller name="arm_hybrid" type="CartesianHybridControllerNode" />'
+ resp = kill_and_spawn(hybrid_config, ['arm_pose'])
+ if len(resp.add_name) == 0 or not resp.add_ok[0]:
+ raise "Failed to spawn the hybrid controller"
+
+ ###### Sets the tool frame
+
+ print "Setting tool frame"
+ rospy.wait_for_service("/arm_hybrid/set_tool_frame")
+ if rospy.is_shutdown(): return
+ set_tool_frame = rospy.ServiceProxy("/arm_hybrid/set_tool_frame", SetPoseStamped)
+ set_tool_frame(plug_pose)
+ print "Tool frame set"
+ time.sleep(1)
+
+ ###### Visual differencing
+
+ track_state = Tracker("/arm_hybrid/state", CartesianState)
+
+ pub_hybrid = rospy.Publisher('/arm_hybrid/command', TaskFrameFormalism)
+ tff = TaskFrameFormalism()
+ tff.header.frame_id = 'outlet_pose'
+ tff.mode.vel.x = 3
+ tff.mode.vel.y = 3
+ tff.mode.vel.z = 3
+ tff.mode.rot.x = 3
+ tff.mode.rot.y = 3
+ tff.mode.rot.z = 3
+
+ while not rospy.is_shutdown():
+ time.sleep(0.1)
+ return
+
+ while not rospy.is_shutdown():
+
+ # Waits for the controller to settle
+ vel = 1
+ track_state.msg = None
+ while vel > 0.0001:
+ while not track_state.msg:
+ time.sleep(0.01)
+ if rospy.is_shutdown(): return
+ twist_msg = track_state.msg.last_twist_meas
+ v = math.sqrt(twist_msg.vel.x**2 + twist_msg.vel.y**2 + twist_msg.vel.z**2) + \
+ 0.1 * math.sqrt(twist_msg.rot.x**2 + twist_msg.rot.y**2 + twist_msg.rot.z**2)
+ vel = max(v, 0.1*v + 0.9*vel)
+ print "Vel:", vel
+ print "Settled"
+
+ tool_pose = track_state.msg.last_pose_meas
+
+ # Gets the next plug pose estimate
+ track_plug_pose.msg = None
+ while not track_plug_pose.msg:
+ time.sleep(0.01)
+ print "Got a new plug pose"
+
+ #TODO: plug_in_outlet = TF
+
+ # Determines the offset
+ #set_tool_frame(track_plug_pose.msg)
+
+ # Commands the controller to move
+ pub_hybrid.publish(tff)
+
+
+if __name__ == '__main__': main()
Property changes on: pkg/trunk/demos/plug_in/plug_in2.py
___________________________________________________________________
Added: svn:executable
+ *
Added: pkg/trunk/demos/plug_in/src/servo.cpp
===================================================================
--- pkg/trunk/demos/plug_in/src/servo.cpp (rev 0)
+++ pkg/trunk/demos/plug_in/src/servo.cpp 2009-03-17 00:56:35 UTC (rev 12564)
@@ -0,0 +1,304 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived from
+ * this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include "boost/scoped_ptr.hpp"
+#include "ros/node.h"
+#include "tf/transform_listener.h"
+#include "tf/message_notifier.h"
+
+#include "robot_msgs/PoseStamped.h"
+#include "robot_msgs/CartesianState.h"
+#include "robot_msgs/TaskFrameFormalism.h"
+
+enum {MEASURING, MOVING, PUSHING, FORCING};
+int g_state = MEASURING;
+
+ros::Time g_started_pushing, g_started_forcing;
+
+bool g_settled = false;
+robot_msgs::CartesianState g_hybrid_state;
+void hybrid_state_cb()
+{
+ static double speed = 1.0;
+
+ tf::Vector3 vel, rot;
+ tf::PointMsgToTF(g_hybrid_state.last_twist_meas.vel, vel);
+ tf::PointMsgToTF(g_hybrid_state.last_twist_meas.rot, rot);
+
+ double s = vel.length() + 0.05 * rot.length();
+ speed = s > speed ? s : (0.05 * s + 0.95 * speed);
+
+ //printf("Speed: %lf\n", speed);
+ if (!g_settled && speed < 0.4)
+ {
+ printf("Settled\n");
+ g_settled = true;
+ }
+ if (g_settled && speed > 0.5)
+ {
+ printf("Moving again\n");
+ g_settled = false;
+ }
+}
+
+boost::scoped_ptr<tf::TransformListener> TF;
+
+//robot_msgs::PoseStamped g_plug;
+void plug_cb(const tf::MessageNotifier<robot_msgs::PoseStamped>::MessagePtr &msg)
+{
+ if (!g_settled) {
+ printf("Not yet settled\n");
+ return;
+ }
+
+ // Both are transforms from the outlet to the estimated plug pose
+ robot_msgs::PoseStamped viz_offset_msg;
+ tf::Stamped<tf::Transform> mech_offset;
+ try {
+ TF->transformPose("outlet_pose", *(msg.get()), viz_offset_msg);
+ TF->lookupTransform("outlet_pose", "arm_hybrid/tool_frame", msg->header.stamp, mech_offset);
+ }
+ catch(tf::TransformException &ex)
+ {
+ printf("FAIL: %s\n", ex.what());
+ return;
+ }
+
+ tf::Pose viz_offset;
+ tf::PoseMsgToTF(viz_offset_msg.pose, viz_offset);
+
+ // The standoff is at least half the current distance
+ double standoff = std::max(0.03, viz_offset.getOrigin().length() / 2.0);
+
+ // Computes the offset for movement
+ tf::Pose viz_offset_desi, mech_offset_desi;
+ viz_offset_desi.setIdentity();
+ viz_offset_desi.getOrigin().setX(-standoff);
+ //mech_offset_desi = mech_offset + (viz_offset_desi - viz_offset);
+ //mech_offset_desi = mech_offset * viz_offset.inverse() * viz_offset_desi;
+ mech_offset_desi = viz_offset.inverse() * viz_offset_desi * mech_offset;
+
+
+ switch (g_state) {
+ case MEASURING: {
+ printf("standoff = %.3lf\n", standoff);
+ printf("Mech offset: (% .3lf, % .3lf, % .3lf) <% .2lf, % .2lf, % .2lf, % .2lf>\n",
+ mech_offset.getOrigin().x(),
+ mech_offset.getOrigin().y(),
+ mech_offset.getOrigin().z(),
+ mech_offset.getRotation().x(),
+ mech_offset.getRotation().y(),
+ mech_offset.getRotation().z(),
+ mech_offset.getRotation().w());
+ printf("Viz offset: (% .3lf, % .3lf, % .3lf) <% .2lf, % .2lf, % .2lf, % .2lf>\n",
+ viz_offset.getOrigin().x(),
+ viz_offset.getOrigin().y(),
+ viz_offset.getOrigin().z(),
+ viz_offset.getRotation().x(),
+ viz_offset.getRotation().y(),
+ viz_offset.getRotation().z(),
+ viz_offset.getRotation().w());
+ printf("Command: (% .3lf, % .3lf, % .3lf) <% .2lf, % .2lf, % .2lf, % .2lf>\n",
+ mech_offset_desi.getOrigin().x(),
+ mech_offset_desi.getOrigin().y(),
+ mech_offset_desi.getOrigin().z(),
+ mech_offset_desi.getRotation().x(),
+ mech_offset_desi.getRotation().y(),
+ mech_offset_desi.getRotation().z(),
+ mech_offset_desi.getRotation().w());
+
+ if (viz_offset.getOrigin().length() > 0.5 ||
+ viz_offset.getRotation().getAngle() > (M_PI/4.0))
+ {
+ printf("WOAH! Crazy vision offset!!!\n");
+ g_state = MEASURING;
+ break;
+ }
+
+ // TODO: check (mechanism) velocities and throw out measurements taken while moving
+
+ double error = sqrt(pow(viz_offset.getOrigin().y(), 2) + pow(viz_offset.getOrigin().z(), 2));
+ printf("error = %0.6lf\n", error);
+ if (error < 0.002)
+ {
+ printf("enter PUSHING\n");
+ g_state = PUSHING;
+ g_started_pushing = ros::Time::now();
+ robot_msgs::TaskFrameFormalism tff;
+ tff.header.frame_id = "outlet_pose";
+ tff.header.stamp = msg->header.stamp;
+ tff.mode.vel.x = 2;
+ tff.mode.vel.y = 3;
+ tff.mode.vel.z = 3;
+ tff.mode.rot.x = 3;
+ tff.mode.rot.y = 3;
+ tff.mode.rot.z = 3;
+ tff.value.vel.x = 0.6;
+ tff.value.vel.y = mech_offset_desi.getOrigin().y();
+ tff.value.vel.z = mech_offset_desi.getOrigin().z();
+ mech_offset_desi.getBasis().getEulerZYX(tff.value.rot.z, tff.value.rot.y, tff.value.rot.x);
+ ros::Node::instance()->publish("/arm_hybrid/command", tff);
+ }
+ else
+ {
+ g_state = MOVING;
+ robot_msgs::TaskFrameFormalism tff;
+ tff.header.frame_id = "outlet_pose";
+ tff.header.stamp = msg->header.stamp;
+ tff.mode.vel.x = 3;
+ tff.mode.vel.y = 3;
+ tff.mode.vel.z = 3;
+ tff.mode.rot.x = 3;
+ tff.mode.rot.y = 3;
+ tff.mode.rot.z = 3;
+ tff.value.vel.x = mech_offset_desi.getOrigin().x();
+ tff.value.vel.y = mech_offset_desi.getOrigin().y();
+ tff.value.vel.z = mech_offset_desi.getOrigin().z();
+ mech_offset_desi.getBasis().getEulerZYX(tff.value.rot.z, tff.value.rot.y, tff.value.rot.x);
+ ros::Node::instance()->publish("/arm_hybrid/command", tff);
+ }
+
+ break;
+ }
+
+ case MOVING: {
+ // TODO: Wait to leave MOVING until settled
+ g_state = MEASURING;
+ break;
+ }
+
+ case PUSHING: {
+
+ // TODO: When do we switch to FORCING?
+ // - Push until we stop making forward progress
+ // - Verify that we are in the outlet
+ // But for now, we just make sure we've gone for a few seconds
+ if (g_started_pushing + ros::Duration(3.0) > ros::Time::now())
+ {
+ if (true) // if (in_outlet)
+ {
+ printf("enter FORCING\n");
+ g_state = FORCING;
+ g_started_forcing = ros::Time::now();
+ robot_msgs::TaskFrameFormalism tff;
+ tff.header.frame_id = "outlet_pose";
+ tff.header.stamp = msg->header.stamp;
+ tff.mode.vel.x = 1;
+ tff.mode.vel.y = 3;
+ tff.mode.vel.z = 3;
+ tff.mode.rot.x = 2;
+ tff.mode.rot.y = 2;
+ tff.mode.rot.z = 2;
+ tff.value.vel.x = 40;
+ tff.value.vel.y = mech_offset.getOrigin().y();
+ tff.value.vel.z = mech_offset.getOrigin().z();
+ tff.value.rot.x = 0.0;
+ tff.value.rot.y = 0.0;
+ tff.value.rot.z = 0.0;
+
+
+ tff.mode.vel.y = 2;
+ tff.mode.vel.z = 2;
+ tff.value.vel.y = 0.0;
+ tff.value.vel.z = 0.0;
+ ros::Node::instance()->publish("/arm_hybrid/command", tff);
+ }
+ else
+ {
+ printf("enter MEASURING\n");
+ g_state = MEASURING;
+ robot_msgs::TaskFrameFormalism tff;
+ tff.header.frame_id = "outlet_pose";
+ tff.header.stamp = msg->header.stamp;
+ tff.mode.vel.x = 3;
+ tff.mode.vel.y = 3;
+ tff.mode.vel.z = 3;
+ tff.mode.rot.x = 3;
+ tff.mode.rot.y = 3;
+ tff.mode.rot.z = 3;
+ tff.value.vel.x = mech_offset.getOrigin().x() - 0.05; // backs off 5cm
+ tff.value.vel.y = mech_offset.getOrigin().y();
+ tff.value.vel.z = mech_offset.getOrigin().z();
+ mech_offset.getBasis().getEulerZYX(tff.value.rot.z, tff.value.rot.y, tff.value.rot.x);
+ ros::Node::instance()->publish("/arm_hybrid/command", tff);
+ }
+ }
+
+ break;
+ }
+
+ case FORCING: {
+
+ // TODO: Get out of here if forcing is failing
+ if (g_started_forcing + ros::Duration(5.0) > ros::Time::now())
+ {
+ printf("enter MEASURING\n");
+ g_state = MEASURING;
+ robot_msgs::TaskFrameFormalism tff;
+ tff.header.frame_id = "outlet_pose";
+ tff.header.stamp = msg->header.stamp;
+ tff.mode.vel.x = 3;
+ tff.mode.vel.y = 3;
+ tff.mode.vel.z = 3;
+ tff.mode.rot.x = 3;
+ tff.mode.rot.y = 3;
+ tff.mode.rot.z = 3;
+ tff.value.vel.x = mech_offset.getOrigin().x() - 0.06; // backs off
+ tff.value.vel.y = mech_offset.getOrigin().y();
+ tff.value.vel.z = mech_offset.getOrigin().z();
+ mech_offset.getBasis().getEulerZYX(tff.value.rot.z, tff.value.rot.y, tff.value.rot.x);
+ ros::Node::instance()->publish("/arm_hybrid/command", tff);
+ }
+
+
+ break;
+ }
+ }
+
+}
+
+
+int main(int argc, char** argv)
+{
+ ros::init(argc, argv);
+ ros::Node node("servo");
+
+ node.advertise<robot_msgs::TaskFrameFormalism>("/arm_hybrid/command", 2);
+
+ TF.reset(new tf::TransformListener(node));
+ //tf::TransformListener TF(node);
+
+ node.subscribe("/arm_hybrid/state", g_hybrid_state, hybrid_state_cb, 2);
+ //node.subscribe("/plug_detector/pose", g_plug, plug_cb, 2);
+ tf::MessageNotifier<robot_msgs::PoseStamped> mn(
+ TF.get(), &node, plug_cb, "/plug_detector/pose", "outlet_pose", 5);
+ node.spin();
+
+ return 0;
+}
Added: pkg/trunk/prcore/robot_msgs/msg/CartesianState.msg
===================================================================
--- pkg/trunk/prcore/robot_msgs/msg/CartesianState.msg (rev 0)
+++ pkg/trunk/prcore/robot_msgs/msg/CartesianState.msg 2009-03-17 00:56:35 UTC (rev 12564)
@@ -0,0 +1,7 @@
+Header header
+robot_msgs/PoseStamped last_pose_meas
+robot_msgs/Twist last_pose_desi
+robot_msgs/Twist last_twist_meas
+#last_pose_desi
+robot_msgs/Twist last_twist_desi
+robot_msgs/Wrench last_wrench_desi
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
