$search

articulate_cart_server.py

Go to the documentation of this file.

#! /usr/bin/env python
# Copyright (c) 2008, Willow Garage, Inc.
# All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
#         * Redistributions of source code must retain the above copyright
#             notice, this list of conditions and the following disclaimer.
#         * 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.
#         * Neither the name of the Willow Garage, Inc. 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.

# Author: Jon Scholz

PKG="articulate_cart"
import roslib; roslib.load_manifest(PKG)
import rospy
import tf
import thread
import numpy
import threading

from tf import transformations


from std_msgs.msg import *
from std_msgs.msg import Bool
from geometry_msgs.msg import *
#from pr2_controllers_msgs.msg import *
from math import pi, pow, atan, sin, cos, sqrt

class ArticulateCartException(Exception):
  pass

class articulateCartServer(object):
  """ A server for manipulating a cart being grasped by the grippers
  of the pr2.  Uses manipulation_transforms to compute gripper poses satisfying
  incoming goal requests.  Goals for the cart pose are specified in the
  base_footprint frame.  manipulation_transforms is used to map this goal to
  gripper poses in the base_footprint frame, which are then written
  to the goal topics for teleop_controllers (make sure to specify base_footprint
  as the frame_id rather than torso_lift_link).
  Accepts goals as PoseStamped messages, and issues PoseStamped messages
  to the arms.
  """

  # create messages that are used to publish feedback/result
  _r_arm_SSE = 0
  _l_arm_SSE = 0
  _r_arm_err_twist = Twist()
  _l_arm_err_twist = Twist()
  errthresh = 0.02

  ##
  # @param tf_object_name: if non-empty, attempt to lookup object initial pose on tf with given frame name, and re-initialize manipulation_transforms 
  #
  def __init__(self, tf_object_name):
    self._tf_object_name       = tf_object_name
      
    # A tf listener for reading gripper poses and stuff
    self._tl = tf.TransformListener()

    # Indicator fake twist controller loop
    self._fake_twist_loop_started = False # Toggled by first incoming twist message

    self._last_warn_infeasible = 0

    ## Mutex for pose-related members {_cart_pose_tup, _cart_pose_msg, _pose_resp, _twist_req, _cart_pose_euler}
    self._pose_mutex = threading.RLock()

    self._last_twist_req = rospy.Time(0)

    self._r_xfm = None
    self._l_xfm = None

    ## Load cart workspace constraints from parameter server
    self._cart_x_min = rospy.get_param("cart_pushing/x_min")
    self._cart_x_max = rospy.get_param("cart_pushing/x_max")
    self._cart_y_min = rospy.get_param("cart_pushing/y_min")
    self._cart_y_max = rospy.get_param("cart_pushing/y_max")
    self._cart_t_min = rospy.get_param("cart_pushing/t_min")
    self._cart_t_max = rospy.get_param("cart_pushing/t_max")

    ## Hard-code a set of arm postures
    self._postures = {
      'off': [],
      'mantis': [0, 1, 0,  -1, 3.14, -1, 3.14],
      'elbowupr': [-0.79,0,-1.6,  9999, 9999, 9999, 9999],
      'elbowupl': [0.79,0,1.6 , 9999, 9999, 9999, 9999],
      'old_elbowupr': [-0.79,0,-1.6, -0.79,3.14, -0.79,5.49],
      'old_elbowupl': [0.79,0,1.6, -0.79,3.14, -0.79,5.49],
      'elbowdownr': [-0.028262077316910873, 1.2946342642324222, -0.25785640577652386, 
                      -1.5498884526859626, -31.278913849571776, -1.0527644894829107, -1.8127318367654268],
      'elbowdownl': [-0.0088195719039858515, 1.2834828245284853, 0.20338442004843196, 
                      -1.5565279256852611, -0.096340012666916802, -1.0235018652439782, 1.7990893054129216],
      'elbowmidr': [-0.28990347455256005, 1.089312348420358, -0.94309188122730947, -1.3356881736879038, 
                     0.42316056966725524, -1.3491191190220519, -5.3263966597959937],
      'elbowmidl': [0.52521589892944154, 1.0820586985900948, 1.0342420867971822, -1.3718169587386895, 
                    -6.6154390746853364, -1.3118240320657639, -10.572623351665454]
      }


    ## Attach to all topics and services
    self._initialize_ros_coms()

    self._active = False
    self._active_sub = rospy.Subscriber('~articulate_cart', Bool, self._active_cb)
    
    self._active_pub = rospy.Publisher('~articulation_active', Bool, latch=True)
    self._active_pub.publish(self._active)
    
    ## Initialize object and gripper poses according to constructor params
    self._initialize_poses()
        


    ## Spin thread for posture updater
    thread.start_new_thread(self._posture_loop, ("elbowdownr", "elbowdownl")) # elbowmidr, elbowmidl
    
    ## All done:
    print "Articulate Cart Server Ready!"
    
  def _active_cb(self, msg):
      rospy.loginfo('Cart articulation status set to %s' % (('ENABLED' if msg.data else 'DISABLED')))
      self._active = msg.data
      self._active_pub.publish(self._active)
      
      if self._active:
          self._initialize_poses()
      else:
          # Reset postures
          self._r_posture_pub.publish(Float64MultiArray())
          self._l_posture_pub.publish(Float64MultiArray())

  def _initialize_ros_coms(self):
    ## subscriber for goal topic
    self._goal_sub = rospy.Subscriber('~command_twist', TwistStamped, self._fake_twist_goal_cb)

    
    ## publisher for arm cartesian pose/twist controller
    self._r_arm_pose_pub = rospy.Publisher('/r_cart/command_pose', PoseStamped)
    self._l_arm_pose_pub = rospy.Publisher('/l_cart/command_pose', PoseStamped)
    self._r_arm_twist_pub = rospy.Publisher('/r_cart/command_twist', TwistStamped)
    self._l_arm_twist_pub = rospy.Publisher('/l_cart/command_twist', TwistStamped)
    
    ## Publishers for teleop_controllers' posture topics
    self._r_posture_pub = rospy.Publisher("/r_cart/command_posture", Float64MultiArray)
    self._l_posture_pub = rospy.Publisher("/l_cart/command_posture", Float64MultiArray)

    ## Publisher for when when an invalid pose is received
    self.invalid_pose_pub = rospy.Publisher("~invalid_pose", Empty)
    
    ## subscriber for arm state errors
    self._r_arm_sub = rospy.Subscriber('/r_cart/state/x_err', Twist, self._r_arm_err_cb)
    self._l_arm_sub = rospy.Subscriber('/l_cart/state/x_err', Twist, self._l_arm_err_cb)
    self._r_arm_err_lock = thread.allocate_lock()
    self._l_arm_err_lock = thread.allocate_lock()
    
      
  def _initialize_poses(self):
      r_pose_tup = self.get_tf_transform('base_footprint', 'r_gripper_tool_frame')
      l_pose_tup = self.get_tf_transform('base_footprint', 'l_gripper_tool_frame')
      cart_pose_tup = self.get_tf_transform('base_footprint', self._tf_object_name)

      if not r_pose_tup or not l_pose_tup or not cart_pose_tup:
          rospy.logerr('Unable to find initial cart and gripper poses!')
          self._r_xfm = None
          self._l_xfm = None
          return False

      # Convert to matrices
      r_pose_mtx = transformations.compose_matrix(translate = r_pose_tup[0], angles = transformations.euler_from_quaternion(r_pose_tup[1]))
      l_pose_mtx = transformations.compose_matrix(translate = l_pose_tup[0], angles = transformations.euler_from_quaternion(l_pose_tup[1]))
      cart_pose_mtx = transformations.compose_matrix(translate = cart_pose_tup[0], angles = transformations.euler_from_quaternion(cart_pose_tup[1]))

      self._r_xfm = numpy.dot(r_pose_mtx, numpy.linalg.inv(cart_pose_mtx))
      self._l_xfm = numpy.dot(l_pose_mtx, numpy.linalg.inv(cart_pose_mtx))
      
      rospy.loginfo('Cart, gripper poses initialized')
      
      return True

  def _fake_twist_goal_cb(self, msg):
    """An alternative callback for twist messages that spawns an internal controller to simulate
    the object twist action using pose commands"""
    rospy.logdebug('Read goal: [%f,%f,%f][%f,%f,%f]', 
                  msg.twist.linear.x,
                  msg.twist.linear.y,
                  msg.twist.linear.z,
                  msg.twist.angular.x,
                  msg.twist.angular.y,
                  msg.twist.angular.z)
    
    # Save twist request for fake controller
    with self._pose_mutex:
      self._twist_req = msg.twist
      self._last_twist_req = msg.header.stamp

    # if this is the first incoming message, spin the controller thread
    if self._fake_twist_loop_started == False:
        self._fake_twist_loop_started = True;
        thread.start_new_thread(self._fake_twist_controller, ()) # at default frequency
      
  def _r_arm_err_cb(self, twist):
    self._r_arm_err_lock.acquire()
    self._r_arm_err_twist=twist;
    self._r_arm_SSE = self.get_twist_SSE(twist)
    self._r_arm_err_lock.release()

  def _l_arm_err_cb(self, twist):
    self._l_arm_err_lock.acquire()
    self._l_arm_err_twist=twist
    self._l_arm_SSE = self.get_twist_SSE(twist)
    self._l_arm_err_lock.release()

  def _posture_loop(self, r_posture, l_posture):
    """ Send updates to the posture topic every ROS second """
    while not rospy.is_shutdown():
        if self._active:
            self._r_posture_pub.publish(Float64MultiArray(data = self._postures[r_posture]))
            self._l_posture_pub.publish(Float64MultiArray(data = self._postures[l_posture]))
        rospy.sleep(1.0)

  def _fake_twist_controller(self, r = 20, recovery_horizon = 5):
      """ Simulates the twist applied to the object, and updates the gripper poses accordingly.
      """

      counter = 0

      rate = rospy.Rate(r)
      t = 1.0/r

      while not rospy.is_shutdown():
          with self._pose_mutex:
            # Don't command cart if our last update is stale
            if (rospy.get_time() - self._last_twist_req.to_sec() > 1.0):
                rate.sleep()
                continue
              
            # if counter % 200 == 0:
            #  rospy.loginfo('Cart rel pose is %s', self._cart_pose_euler)
            counter += 1
            
            if self._r_xfm is None and self._l_xfm is None and not self._initialize_poses():
                continue
                
            # Load object orientation from tf
            cart_pose_tup = self.get_tf_transform('base_footprint', self._tf_object_name)
        
            if not cart_pose_tup:
                rospy.logerr('Unable to find cart pose in base footprint frame!')
                continue
        
            cart_pose_euler = self.pose_tup_to_euler_list(cart_pose_tup[0], cart_pose_tup[1])

            if self._check_cart_pose(cart_pose_euler):
              # Default case: current cart pose is valid; let's make sure it stays that way
              new_pose = forward_simulate_twist(cart_pose_euler, self._twist_req, t)
              if self._check_cart_pose(new_pose):
                self.set_new_pose(new_pose)
              else:
                self.invalid_pose_pub.publish()
                if rospy.get_time() - self._last_warn_infeasible > 1.0:
                    rospy.logwarn('Applying %s for %s secs from %s would lead to infeasible pose %s; ignoring.',
                                  self._twist_req, t, cart_pose_euler, new_pose)
                    self._last_warn_infeasible = rospy.get_time()

            else:
              # 'Recovery' case: current pose is not valid; we check if the twist is bringing
              # us back into validity
              succeeded = False
              for i in xrange(recovery_horizon):
                new_pose = forward_simulate_twist(cart_pose_euler, self._twist_req, i*t)
                if self._check_cart_pose(new_pose):
                  # Twist is bringing us back to validity, so we accept it
                  rospy.loginfo('Current pose %s invalid, but %s leads to valid pose %s after %s seconds',
                                cart_pose_euler, self._twist_req, new_pose, i*t)
                  self.set_new_pose(new_pose)
                  succeeded = True
                  break
              if not succeeded:
                self.invalid_pose_pub.publish()
                rospy.logwarn('Currently in collision at %s, and twist %s not helping',
                              cart_pose_euler, self._twist_req)
          rate.sleep()

  # Sends commands to arm
  def set_new_pose (self, p):
      cart_pos_mtx = transformations.compose_matrix(translate=p[0], angles=p[1])
      r_desired = _matrix_to_pose(numpy.dot(self._r_xfm, cart_pos_mtx))
      l_desired = _matrix_to_pose(numpy.dot(self._l_xfm, cart_pos_mtx))

      self._pose_command_action(r_desired, l_desired)
      
  def _pose_command_action(self, r_desired, l_desired):
    """Send the target effector poses to the arm control topics
    Assumes that self._pose_resp is up-to-date"""
    if self._active:
        r_ps = PoseStamped(pose=r_desired)
        r_ps.header.stamp = rospy.get_rostime()
        r_ps.header.frame_id = 'base_footprint'

        l_ps = PoseStamped(pose=l_desired)
        l_ps.header.stamp = rospy.get_rostime()
        l_ps.header.frame_id = 'base_footprint'

        self._r_arm_pose_pub.publish(r_ps)
        self._l_arm_pose_pub.publish(l_ps)
    
    #print self._pose_resp.effector_poses
    # Uncomment to wait for error to drop before returning
    # while self._l_arm_SSE + self._r_arm_SSE > self.errthresh and not rospy.is_shutdown():
    #   print self._l_arm_SSE + self._r_arm_SSE

  def get_transform_param(self, name):
      """Returns the named transform as a pose tuple
      
      Arguments:
      - `name`: string matching a pose on the param server
      """
      param = rospy.get_param(name)
      rospy.loginfo('Parameter %s is %s', name, param)
      return (tuple(param["position"]), tuple(param["orientation"]))  
  

  def set_transform_param(self, name, pose_tup):
      """Sets the provided transform param tuple on the paramater server in "cart format"
      
      Arguments:
      - `pose_tup`: a tuple containing ((trans),(rot))
      - `name`: a string to set
      """
      rospy.set_param(name, {"position":pose_tup[0], "orientation":pose_tup[1]})
  
  def pose_tup_to_matrix(self, tup):
      """Returns a numpy 4x4 transform array from a pose provided in a tuple ((vector), (quaternion))"""
      mtr = tf.transformations.quaternion_matrix(tup[1])
      mtr[0][3] = tup[0][0]
      mtr[1][3] = tup[0][1]
      mtr[2][3] = tup[0][2]
      return mtr

  def pose_tup_to_msg(self, tup):
    """Returns Pose message assembled from tf query tuple """
    msg = Pose()
    msg.position.x = tup[0][0]
    msg.position.y = tup[0][1]
    msg.position.z = tup[0][2]
    msg.orientation.x = tup[1][1]
    msg.orientation.y = tup[1][2]
    msg.orientation.z = tup[1][2]
    msg.orientation.w = tup[1][3]
    return msg
  
  def get_rel_pose(self, p1, p2):
      """Compute relative pose of p2 in frame of p1 (4x4 numpy arrays)"""      
      return numpy.dot(numpy.linalg.inv(p1), p2)
  
  def pose_tup_to_euler_list(self, position, orientation):
      """Returns a list representation of Pose [[vector],[euler angles]]"""
      return [position,
               list(tf.transformations.euler_from_quaternion([orientation[0],
                                                              orientation[1],
                                                              orientation[2],
                                                              orientation[3]]))]

  def pose_msg_to_euler_list(self, Pose):
      """Returns a list representation of Pose [[vector],[euler angles]]"""
      return [[Pose.position.x,
               Pose.position.y,
               Pose.position.z],
               list(tf.transformations.euler_from_quaternion([Pose.orientation.x,
                                                              Pose.orientation.y,
                                                              Pose.orientation.z,
                                                              Pose.orientation.w]))]
  def euler_list_to_pose_msg(self, euler_pose):
      """ convert to pose represented as a list [[vector],[euler angles]] to a Pose msg"""
      quat = tf.transformations.quaternion_from_euler(*euler_pose[1])
      return Pose(position=Point(*euler_pose[0]),
                  orientation=Quaternion(*quat))
      
  def twist_msg_to_tup(self, msg):
      """Returns a twist tuple ((linear), (angular)) from parameters in a Twist message """
      return ((msg.linear.x, msg.linear.y, msg.linear.z),
              (msg.angular.x, msg.angular.y, msg.angular.z))

  def twist_tup_to_msg(self, tup):
      """ Returns a Twist message from parameters in a tuple ((linear), (angular))"""
      msg=Twist()
      msg.linear.x = tup[0][0]
      msg.linear.y = tup[0][1]
      msg.linear.z = tup[0][2]
      msg.angular.x = tup[1][0]
      msg.angular.y = tup[1][1]
      msg.angular.z = tup[1][2]  
      return msg
        
  def interpolate_pose_tup(self, p1, p2, blend=0.5):
      """Returns weighted interpolation of two pose tuples ((vector), (quaternion))"""
      linear = blend*numpy.array(list(p1[0])) + (1-blend)*numpy.array(list(p2[0]))
      angular = tf.transformations.quaternion_slerp(p1[1], p2[1], blend)
      return (linear.tolist(), angular.tolist())
  
  def interpolate_twist_tup(self, t1, t2, blend=0.5):
      """Returns a weighted average of two twist tuples ((linear), (angular))"""
      linear = blend*numpy.array(list(t1[0])) + (1-blend)*numpy.array(list(t2[0]))
      angular = blend*numpy.array(list(t1[1])) + (1-blend)*numpy.array(list(t2[1]))
      return (linear.tolist(), angular.tolist())

  def get_tf_transform(self, ref_frame, target_frame):
      """An exception-handling method for querying a transform from tf"""
      try:
          self._tl.waitForTransform(ref_frame, target_frame, rospy.Time(0), rospy.Duration(0.2))
          return self._tl.lookupTransform(ref_frame, target_frame, rospy.Time(0)) 
      except (tf.Exception, tf.LookupException, tf.ConnectivityException) as ex:
          print ex
          return None
  
  def get_l_drift_twist(self):
      """ Uses pose call to solver to compute error at left gripper, and returns as twist
      """
      #TODO: debug this
      
      # Compute target pose for left arm
      #r_pose_tup = self.get_tf_transform('base_footprint', 'r_gripper_tool_frame')
      #r_pose_mtr = self.pose_tup_to_matrix(r_pose_tup)
      #l_pose_goal =  
      
      # Compute relative transform to actual pose 
      ##########################################################
      # Compute actual relative pose of l_gripper in r_gripper frame
      #r_pose_tup = self.get_tf_transform('base_footprint', 'r_gripper_tool_frame')
      #l_pose_tup = self.get_tf_transform('base_footprint', 'l_gripper_tool_frame')
      #r_pose_mtr = self.pose_tup_to_matrix(r_pose_tup)
      #l_pose_mtr = self.pose_tup_to_matrix(l_pose_tup)
      #l_relpose_actual = self.get_rel_pose(r_pose_mtr, l_pose_mtr)
      
      # Compute transform from expected to actual
      #l_pose_err = self.get_rel_pose(self._l_relpose, l_relpose_actual)
      
      # Convert to euler for interpretation as a twist tuple      
      #return (tuple(tf.transformations.translation_from_matrix(l_pose_err)),
      #        tf.transformations.euler_from_matrix(l_pose_err))
      return ((0,0,0),(0,0,0))
      
  def get_twist_SSE(self, twist):
    return sum([pow(twist.linear.x,2),
               pow(twist.linear.y,2),
               pow(twist.linear.z,2),
               pow(twist.angular.x,2),
               pow(twist.angular.y,2),
               pow(twist.angular.z,2)])
  
  def _check_cart_pose(self, pose):
    """ Returns true if pose is reachable given workspace constraints of cart"""
    if pose[0][0] < self._cart_x_min or pose[0][0] > self._cart_x_max or \
       pose[0][1] < self._cart_y_min or pose[0][1] > self._cart_y_max or \
       pose[1][2] < self._cart_t_min or pose[1][2] > self._cart_t_max:
       return False
    
    return True


############################################################
# Non-member utility functions
############################################################

def forward_simulate_twist (pose, twist, t):
    """Forward simulate twist from pose for t seconds and return the resulting pose"""
    return [[pose[0][0] + twist.linear.x*t,
             pose[0][1] + twist.linear.y*t,
             pose[0][2] + twist.linear.z*t],
            [pose[1][0] + twist.angular.x*t,
             pose[1][1] + twist.angular.y*t,
             pose[1][2] + twist.angular.z*t]]

def _matrix_to_pose(mtx):
    trans = transformations.decompose_matrix(mtx)[3]
    
    ps = Pose()
    ps.position.x = trans[0]
    ps.position.y = trans[1]
    ps.position.z = trans[2]

    qt = transformations.quaternion_from_matrix(mtx)
    ps.orientation.x = qt[0]
    ps.orientation.y = qt[1]
    ps.orientation.z = qt[2]
    ps.orientation.w = qt[3]
    
    return ps

############################################################
# Node
############################################################
  
if __name__ == '__main__':
  from optparse import OptionParser
  parser = OptionParser(usage="")
  parser.add_option("-o", "--object", dest="object", default="cart",
                    help="attempt to load an initial object transform with name OBJECT from tf [default=\"%default\"]")
  (options, args) = parser.parse_args()
  
  rospy.init_node(PKG+"_server")
  
  rospy.sleep(0.5) # init
  server = articulateCartServer(options.object)
  
  rospy.spin()


  

---

articulate_cart
Author(s): Jonathan Scholz
autogenerated on Fri Mar 1 14:44:03 2013