From 17759be84a4ae82198d0450eb8aaa7e27a23febd Mon Sep 17 00:00:00 2001 From: Sagar Doshi Date: Sat, 15 Feb 2020 00:33:09 +0000 Subject: [PATCH] x --- tuck_arms.py | 266 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 266 insertions(+) create mode 100755 tuck_arms.py diff --git a/tuck_arms.py b/tuck_arms.py new file mode 100755 index 0000000..f123f8c --- /dev/null +++ b/tuck_arms.py @@ -0,0 +1,266 @@ +#!/usr/bin/env python + +# Copyright (c) 2013-2015, Rethink Robotics +# All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# 1. Redistributions of source code must retain the above copyright notice, +# this list of conditions and the following disclaimer. +# 2. 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. +# 3. Neither the name of the Rethink Robotics 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. + +""" +Tool to tuck/untuck Baxter's arms to/from the shipping pose +""" +import argparse + +from copy import deepcopy + +import rospy + +from std_msgs.msg import ( + Empty, + Bool, +) + +import baxter_interface + +from baxter_core_msgs.msg import ( + CollisionAvoidanceState, +) +from baxter_interface import CHECK_VERSION + + +class Tuck(object): + def __init__(self, tuck_cmd): + self._done = False + self._limbs = ('left', 'right') + self._arms = { + 'left': baxter_interface.Limb('left'), + 'right': baxter_interface.Limb('right'), + } + self._tuck = tuck_cmd + self._tuck_rate = rospy.Rate(20.0) # Hz + self._tuck_threshold = 0.2 # radians + self._peak_angle = -1.6 # radians + self._arm_state = { + 'tuck': {'left': 'none', 'right': 'none'}, + 'collide': {'left': False, 'right': False}, + 'flipped': {'left': False, 'right': False} + } + self._joint_moves = { + 'tuck': { + 'left': [-1.0, -2.07, 3.0, 2.55, 0.0, 0.01, 0.0], + 'right': [1.0, -2.07, -3.0, 2.55, -0.0, 0.01, 0.0] + }, + 'untuck': { + 'left': [-0.08, -1.0, -1.19, 1.94, 0.67, 1.03, -0.50], + 'right': [0.08, -1.0, 1.19, 1.94, -0.67, 1.03, 0.50] + } + } + self._collide_lsub = rospy.Subscriber( + 'robot/limb/left/collision_avoidance_state', + CollisionAvoidanceState, + self._update_collision, 'left') + self._collide_rsub = rospy.Subscriber( + 'robot/limb/right/collision_avoidance_state', + CollisionAvoidanceState, + self._update_collision, 'right') + self._disable_pub = { + 'left': rospy.Publisher( + 'robot/limb/left/suppress_collision_avoidance', + Empty, queue_size=10), + 'right': rospy.Publisher( + 'robot/limb/right/suppress_collision_avoidance', + Empty, queue_size=10) + } + self._rs = baxter_interface.RobotEnable(CHECK_VERSION) + self._enable_pub = rospy.Publisher('robot/set_super_enable', + Bool, queue_size=10) + + def _update_collision(self, data, limb): + self._arm_state['collide'][limb] = len(data.collision_object) > 0 + self._check_arm_state() + + def _check_arm_state(self): + """ + Check for goals and behind collision field. + + If s1 joint is over the peak, collision will need to be disabled + to get the arm around the head-arm collision force-field. + """ + diff_check = lambda a, b: abs(a - b) <= self._tuck_threshold + for limb in self._limbs: + angles = [self._arms[limb].joint_angle(joint) + for joint in self._arms[limb].joint_names()] + + # Check if in a goal position + untuck_goal = map(diff_check, angles, + self._joint_moves['untuck'][limb]) + tuck_goal = map(diff_check, angles[0:2], + self._joint_moves['tuck'][limb][0:2]) + if all(untuck_goal): + self._arm_state['tuck'][limb] = 'untuck' + elif all(tuck_goal): + self._arm_state['tuck'][limb] = 'tuck' + else: + self._arm_state['tuck'][limb] = 'none' + + # Check if shoulder is flipped over peak + self._arm_state['flipped'][limb] = ( + self._arms[limb].joint_angle(limb + '_s1') <= self._peak_angle) + + def _prepare_to_tuck(self): + # If arms are in "tucked" state, disable collision avoidance + # before enabling robot, to avoid arm jerking from "force-field". + head = baxter_interface.Head() + start_disabled = not self._rs.state().enabled + at_goal = lambda: (abs(head.pan()) <= + baxter_interface.settings.HEAD_PAN_ANGLE_TOLERANCE) + + rospy.loginfo("Moving head to neutral position") + while not at_goal() and not rospy.is_shutdown(): + if start_disabled: + [pub.publish(Empty()) for pub in self._disable_pub.values()] + if not self._rs.state().enabled: + self._enable_pub.publish(True) + head.set_pan(0.0, 0.5, timeout=0) + self._tuck_rate.sleep() + + if start_disabled: + while self._rs.state().enabled == True and not rospy.is_shutdown(): + [pub.publish(Empty()) for pub in self._disable_pub.values()] + self._enable_pub.publish(False) + self._tuck_rate.sleep() + + def _move_to(self, tuck, disabled): + if any(disabled.values()): + [pub.publish(Empty()) for pub in self._disable_pub.values()] + while (any(self._arm_state['tuck'][limb] != goal + for limb, goal in tuck.viewitems()) + and not rospy.is_shutdown()): + if self._rs.state().enabled == False: + self._enable_pub.publish(True) + for limb in self._limbs: + if disabled[limb]: + self._disable_pub[limb].publish(Empty()) + if limb in tuck: + self._arms[limb].set_joint_positions(dict(zip( + self._arms[limb].joint_names(), + self._joint_moves[tuck[limb]][limb]))) + self._check_arm_state() + self._tuck_rate.sleep() + + if any(self._arm_state['collide'].values()): + self._rs.disable() + return + + def supervised_tuck(self): + # Update our starting state to check if arms are tucked + self._prepare_to_tuck() + self._check_arm_state() + # Tuck Arms + if self._tuck == True: + # If arms are already tucked, report this to user and exit. + if all(self._arm_state['tuck'][limb] == 'tuck' + for limb in self._limbs): + rospy.loginfo("Tucking: Arms already in 'Tucked' position.") + self._done = True + return + else: + rospy.loginfo("Tucking: One or more arms not Tucked.") + any_flipped = not all(self._arm_state['flipped'].values()) + if any_flipped: + rospy.loginfo( + "Moving to neutral start position with collision %s.", + "on" if any_flipped else "off") + # Move to neutral pose before tucking arms to avoid damage + self._check_arm_state() + actions = dict() + disabled = {'left': True, 'right': True} + for limb in self._limbs: + if not self._arm_state['flipped'][limb]: + actions[limb] = 'untuck' + disabled[limb] = False + self._move_to(actions, disabled) + + # Disable collision and Tuck Arms + rospy.loginfo("Tucking: Tucking with collision avoidance off.") + actions = {'left': 'tuck', 'right': 'tuck'} + disabled = {'left': True, 'right': True} + self._move_to(actions, disabled) + self._done = True + return + + # Untuck Arms + else: + # If arms are tucked disable collision and untuck arms + if any(self._arm_state['flipped'].values()): + rospy.loginfo("Untucking: One or more arms Tucked;" + " Disabling Collision Avoidance and untucking.") + self._check_arm_state() + suppress = deepcopy(self._arm_state['flipped']) + actions = {'left': 'untuck', 'right': 'untuck'} + self._move_to(actions, suppress) + self._done = True + return + # If arms already untucked, move to neutral location + else: + rospy.loginfo("Untucking: Arms already Untucked;" + " Moving to neutral position.") + self._check_arm_state() + suppress = deepcopy(self._arm_state['flipped']) + actions = {'left': 'untuck', 'right': 'untuck'} + self._move_to(actions, suppress) + self._done = True + return + + def clean_shutdown(self): + """Handles ROS shutdown (Ctrl-C) safely.""" + if not self._done: + rospy.logwarn('Aborting: Shutting down safely...') + if any(self._arm_state['collide'].values()): + while self._rs.state().enabled != False: + [pub.publish(Empty()) for pub in self._disable_pub.values()] + self._enable_pub.publish(False) + self._tuck_rate.sleep() + + +def main(): + parser = argparse.ArgumentParser() + tuck_group = parser.add_mutually_exclusive_group(required=True) + tuck_group.add_argument("-t", "--tuck", dest="tuck", + action='store_true', default=False, help="tuck arms") + tuck_group.add_argument("-u", "--untuck", dest="untuck", + action='store_true', default=False, help="untuck arms") + args = parser.parse_args(rospy.myargv()[1:]) + tuck = args.tuck + + rospy.loginfo("Initializing node... ") + rospy.init_node("rsdk_tuck_arms") + rospy.loginfo("%sucking arms" % ("T" if tuck else "Unt",)) + tucker = Tuck(tuck) + rospy.on_shutdown(tucker.clean_shutdown) + tucker.supervised_tuck() + rospy.loginfo("Finished tuck") + +if __name__ == "__main__": + main()