joint_trajectory_client.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # Copyright (c) 2013-2015, Rethink Robotics
00004 # All rights reserved.
00005 #
00006 # Redistribution and use in source and binary forms, with or without
00007 # modification, are permitted provided that the following conditions are met:
00008 #
00009 # 1. Redistributions of source code must retain the above copyright notice,
00010 #    this list of conditions and the following disclaimer.
00011 # 2. Redistributions in binary form must reproduce the above copyright
00012 #    notice, this list of conditions and the following disclaimer in the
00013 #    documentation and/or other materials provided with the distribution.
00014 # 3. Neither the name of the Rethink Robotics nor the names of its
00015 #    contributors may be used to endorse or promote products derived from
00016 #    this software without specific prior written permission.
00017 #
00018 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00019 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00020 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00021 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00022 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00023 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00024 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00025 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00026 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00027 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00028 # POSSIBILITY OF SUCH DAMAGE.
00029 
00030 """
00031 Baxter RSDK Joint Trajectory Action Client Example
00032 """
00033 import argparse
00034 import sys
00035 
00036 from copy import copy
00037 
00038 import rospy
00039 
00040 import actionlib
00041 
00042 from control_msgs.msg import (
00043     FollowJointTrajectoryAction,
00044     FollowJointTrajectoryGoal,
00045 )
00046 from trajectory_msgs.msg import (
00047     JointTrajectoryPoint,
00048 )
00049 
00050 import baxter_interface
00051 
00052 from baxter_interface import CHECK_VERSION
00053 
00054 
00055 class Trajectory(object):
00056     def __init__(self, limb):
00057         ns = 'robot/limb/' + limb + '/'
00058         self._client = actionlib.SimpleActionClient(
00059             ns + "follow_joint_trajectory",
00060             FollowJointTrajectoryAction,
00061         )
00062         self._goal = FollowJointTrajectoryGoal()
00063         self._goal_time_tolerance = rospy.Time(0.1)
00064         self._goal.goal_time_tolerance = self._goal_time_tolerance
00065         server_up = self._client.wait_for_server(timeout=rospy.Duration(10.0))
00066         if not server_up:
00067             rospy.logerr("Timed out waiting for Joint Trajectory"
00068                          " Action Server to connect. Start the action server"
00069                          " before running example.")
00070             rospy.signal_shutdown("Timed out waiting for Action Server")
00071             sys.exit(1)
00072         self.clear(limb)
00073 
00074     def add_point(self, positions, time):
00075         point = JointTrajectoryPoint()
00076         point.positions = copy(positions)
00077         point.time_from_start = rospy.Duration(time)
00078         self._goal.trajectory.points.append(point)
00079 
00080     def start(self):
00081         self._goal.trajectory.header.stamp = rospy.Time.now()
00082         self._client.send_goal(self._goal)
00083 
00084     def stop(self):
00085         self._client.cancel_goal()
00086 
00087     def wait(self, timeout=15.0):
00088         self._client.wait_for_result(timeout=rospy.Duration(timeout))
00089 
00090     def result(self):
00091         return self._client.get_result()
00092 
00093     def clear(self, limb):
00094         self._goal = FollowJointTrajectoryGoal()
00095         self._goal.goal_time_tolerance = self._goal_time_tolerance
00096         self._goal.trajectory.joint_names = [limb + '_' + joint for joint in \
00097             ['s0', 's1', 'e0', 'e1', 'w0', 'w1', 'w2']]
00098 
00099 
00100 def main():
00101     """RSDK Joint Trajectory Example: Simple Action Client
00102 
00103     Creates a client of the Joint Trajectory Action Server
00104     to send commands of standard action type,
00105     control_msgs/FollowJointTrajectoryAction.
00106 
00107     Make sure to start the joint_trajectory_action_server.py
00108     first. Then run this example on a specified limb to
00109     command a short series of trajectory points for the arm
00110     to follow.
00111     """
00112     arg_fmt = argparse.RawDescriptionHelpFormatter
00113     parser = argparse.ArgumentParser(formatter_class=arg_fmt,
00114                                      description=main.__doc__)
00115     required = parser.add_argument_group('required arguments')
00116     required.add_argument(
00117         '-l', '--limb', required=True, choices=['left', 'right'],
00118         help='send joint trajectory to which limb'
00119     )
00120     args = parser.parse_args(rospy.myargv()[1:])
00121     limb = args.limb
00122 
00123     print("Initializing node... ")
00124     rospy.init_node("rsdk_joint_trajectory_client_%s" % (limb,))
00125     print("Getting robot state... ")
00126     rs = baxter_interface.RobotEnable(CHECK_VERSION)
00127     print("Enabling robot... ")
00128     rs.enable()
00129     print("Running. Ctrl-c to quit")
00130     positions = {
00131         'left':  [-0.11, -0.62, -1.15, 1.32,  0.80, 1.27,  2.39],
00132         'right':  [0.11, -0.62,  1.15, 1.32, -0.80, 1.27, -2.39],
00133     }
00134 
00135     traj = Trajectory(limb)
00136     rospy.on_shutdown(traj.stop)
00137     # Command Current Joint Positions first
00138     limb_interface = baxter_interface.limb.Limb(limb)
00139     current_angles = [limb_interface.joint_angle(joint) for joint in limb_interface.joint_names()]
00140     traj.add_point(current_angles, 0.0)
00141 
00142     p1 = positions[limb]
00143     traj.add_point(p1, 7.0)
00144     traj.add_point([x * 0.75 for x in p1], 9.0)
00145     traj.add_point([x * 1.25 for x in p1], 12.0)
00146     traj.start()
00147     traj.wait(15.0)
00148     print("Exiting - Joint Trajectory Action Test Complete")
00149 
00150 if __name__ == "__main__":
00151     main()


baxter_examples
Author(s): Rethink Robotics Inc.
autogenerated on Sat Jun 8 2019 20:16:27