trajectory_lock.py
Go to the documentation of this file.
1 #! /usr/bin/env python
2 # Copyright (c) 2009, Willow Garage, Inc.
3 # All rights reserved.
4 #
5 # Redistribution and use in source and binary forms, with or without
6 # modification, are permitted provided that the following conditions are met:
7 #
8 # * Redistributions of source code must retain the above copyright
9 # notice, this list of conditions and the following disclaimer.
10 # * Redistributions in binary form must reproduce the above copyright
11 # notice, this list of conditions and the following disclaimer in the
12 # documentation and/or other materials provided with the distribution.
13 # * Neither the name of the Willow Garage, Inc. nor the names of its
14 # contributors may be used to endorse or promote products derived from
15 # this software without specific prior written permission.
16 #
17 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 # POSSIBILITY OF SUCH DAMAGE.
28 
29 # author: Vijay Pradeep
30 
31 
32 # This is a super simple script to change the trajectory setpoint to
33 # the current position of the joints. This is designed to work with
34 # the JointSplineTrajectoryController, and has to live live in the same
35 # namespace as the controller, in order to link up correctly to the
36 # "<controller>/command" and "<controller>/state" topics".
37 
38 import sys
39 import time
40 import rospy
41 
42 from trajectory_msgs.msg import *
43 from pr2_controllers_msgs.msg import *
44 
45 rospy.init_node("trajectory_lock")
46 
47 if len(sys.argv) < 2:
48  print "Using default joint bound of .08 per joint"
49  joint_bounds = [.08]*10
50 else:
51  joint_bounds = [float(x) for x in rospy.myargv()[1:]]
52 
53 def callback(msg):
54  global pub
55 
56  max_error = max([abs(x) for x in msg.error.positions])
57 
58  exceeded = [abs(x) > y for x,y in zip(msg.error.positions, joint_bounds)]
59 
60  print "All: %s" % " ".join(["% .4f" % x for x in msg.error.positions] )
61 
62  if any(exceeded):
63  print "Exceeded: %.4f" % max_error
64 
65  # Copy our current state into the commanded state
66  cmd = trajectory_msgs.msg.JointTrajectory()
67  cmd.header.stamp = msg.header.stamp
68  cmd.joint_names = msg.joint_names
69  cmd.points.append( trajectory_msgs.msg.JointTrajectoryPoint())
70  cmd.points[0].time_from_start = rospy.Duration(.125)
71  cmd.points[0].positions = msg.actual.positions
72  pub.publish(cmd)
73  else:
74  print "Small: %.4f" % max_error
75 
76 global pub
77 pub = rospy.Publisher("command", trajectory_msgs.msg.JointTrajectory, queue_size=10)
78 
79 rospy.Subscriber("state", pr2_controllers_msgs.msg.JointTrajectoryControllerState, callback)
80 
81 rospy.spin()


pr2_mannequin_mode
Author(s): Vijay Pradeep
autogenerated on Thu Jun 6 2019 19:18:43