trajectory_lock.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 # Copyright (c) 2009, Willow Garage, Inc.
00003 # All rights reserved.
00004 #
00005 # Redistribution and use in source and binary forms, with or without
00006 # modification, are permitted provided that the following conditions are met:
00007 #
00008 #     * Redistributions of source code must retain the above copyright
00009 #       notice, this list of conditions and the following disclaimer.
00010 #     * Redistributions in binary form must reproduce the above copyright
00011 #       notice, this list of conditions and the following disclaimer in the
00012 #       documentation and/or other materials provided with the distribution.
00013 #     * Neither the name of the Willow Garage, Inc. nor the names of its
00014 #       contributors may be used to endorse or promote products derived from
00015 #       this software without specific prior written permission.
00016 #
00017 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027 # POSSIBILITY OF SUCH DAMAGE.
00028 
00029 # author: Vijay Pradeep
00030 
00031 
00032 # This is a super simple script to change the trajectory setpoint to
00033 # the current position of the joints.  This is designed to work with
00034 # the JointSplineTrajectoryController, and has to live live in the same
00035 # namespace as the controller, in order to link up correctly to the
00036 # "<controller>/command" and "<controller>/state" topics".
00037 
00038 import roslib
00039 roslib.load_manifest('pr2_mannequin_mode')
00040 import sys
00041 import time
00042 import rospy
00043 
00044 from trajectory_msgs.msg import *
00045 from pr2_controllers_msgs.msg import *
00046 
00047 rospy.init_node("trajectory_lock")
00048 
00049 if len(sys.argv) < 2:
00050     print "Using default joint bound of .08 per joint"
00051     joint_bounds = [.08]*10
00052 else:
00053     joint_bounds = [float(x) for x in rospy.myargv()[1:]]
00054 
00055 def callback(msg):
00056     global pub
00057 
00058     max_error = max([abs(x) for x in msg.error.positions])
00059 
00060     exceeded = [abs(x) > y for x,y in zip(msg.error.positions, joint_bounds)]
00061 
00062     print "All: %s" % "  ".join(["% .4f" % x for x in msg.error.positions] )
00063 
00064     if any(exceeded):
00065         print "Exceeded: %.4f" % max_error
00066 
00067         # Copy our current state into the commanded state
00068         cmd = trajectory_msgs.msg.JointTrajectory()
00069         cmd.header.stamp = msg.header.stamp
00070         cmd.joint_names = msg.joint_names
00071         cmd.points.append( trajectory_msgs.msg.JointTrajectoryPoint())
00072         cmd.points[0].time_from_start = rospy.Duration(.125)
00073         cmd.points[0].positions = msg.actual.positions
00074         pub.publish(cmd)
00075     else:
00076         print "Small: %.4f" % max_error
00077 
00078 global pub
00079 pub = rospy.Publisher("command", trajectory_msgs.msg.JointTrajectory)
00080 
00081 rospy.Subscriber("state", pr2_controllers_msgs.msg.JointTrajectoryControllerState, callback)
00082 
00083 rospy.spin()


pr2_mannequin_mode
Author(s): Vijay Pradeep
autogenerated on Thu Nov 28 2013 11:37:47