267 lines
11 KiB
Python
Executable File
267 lines
11 KiB
Python
Executable File
#!/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()
|