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 sys
00039 import time
00040 import rospy
00041 
00042 from trajectory_msgs.msg import *
00043 from pr2_controllers_msgs.msg import *
00044 
00045 rospy.init_node("trajectory_lock")
00046 
00047 if len(sys.argv) < 2:
00048     print "Using default joint bound of .08 per joint"
00049     joint_bounds = [.08]*10
00050 else:
00051     joint_bounds = [float(x) for x in rospy.myargv()[1:]]
00052 
00053 def callback(msg):
00054     global pub
00055 
00056     max_error = max([abs(x) for x in msg.error.positions])
00057 
00058     exceeded = [abs(x) > y for x,y in zip(msg.error.positions, joint_bounds)]
00059 
00060     print "All: %s" % "  ".join(["% .4f" % x for x in msg.error.positions] )
00061 
00062     if any(exceeded):
00063         print "Exceeded: %.4f" % max_error
00064 
00065         # Copy our current state into the commanded state
00066         cmd = trajectory_msgs.msg.JointTrajectory()
00067         cmd.header.stamp = msg.header.stamp
00068         cmd.joint_names = msg.joint_names
00069         cmd.points.append( trajectory_msgs.msg.JointTrajectoryPoint())
00070         cmd.points[0].time_from_start = rospy.Duration(.125)
00071         cmd.points[0].positions = msg.actual.positions
00072         pub.publish(cmd)
00073     else:
00074         print "Small: %.4f" % max_error
00075 
00076 global pub
00077 pub = rospy.Publisher("command", trajectory_msgs.msg.JointTrajectory)
00078 
00079 rospy.Subscriber("state", pr2_controllers_msgs.msg.JointTrajectoryControllerState, callback)
00080 
00081 rospy.spin()


pr2_mannequin_mode
Author(s): Vijay Pradeep
autogenerated on Thu Jun 6 2019 21:08:04