controller_manager.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 # Software License Agreement (BSD License)
00003 #
00004 # Copyright (c) 2009, Willow Garage, Inc.
00005 # All rights reserved.
00006 #
00007 # Redistribution and use in source and binary forms, with or without
00008 # modification, are permitted provided that the following conditions
00009 # are met:
00010 #
00011 #  * Redistributions of source code must retain the above copyright
00012 #    notice, this list of conditions and the following disclaimer.
00013 #  * Redistributions in binary form must reproduce the above
00014 #    copyright notice, this list of conditions and the following
00015 #    disclaimer in the documentation and/or other materials provided
00016 #    with the distribution.
00017 #  * Neither the name of the Willow Garage nor the names of its
00018 #    contributors may be used to endorse or promote products derived
00019 #    from this software without specific prior written permission.
00020 #
00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 # POSSIBILITY OF SUCH DAMAGE.
00033 #
00034 # author: Kaijen Hsiao
00035 
00036 
00037 ## @package controller_manager
00038 #Interface to various controllers and control services: switches between/uses 
00039 #joint and Cartesian controllers for one arm, controls the corresponding gripper, 
00040 #calls move_arm for collision-free motion planning
00041 
00042 from __future__ import division
00043 import roslib; roslib.load_manifest('pr2_gripper_reactive_approach')
00044 import random, time
00045 import rospy
00046 import tf
00047 from geometry_msgs.msg import PoseStamped, Pose, Point, Quaternion, PointStamped, Vector3Stamped
00048 from pr2_mechanism_msgs.srv import SwitchController, LoadController, UnloadController, ListControllers
00049 import actionlib
00050 from pr2_controllers_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal, \
00051     Pr2GripperCommandGoal, Pr2GripperCommandAction
00052 from pr2_gripper_sensor_msgs.msg import PR2GripperFindContactAction, PR2GripperFindContactGoal, \
00053     PR2GripperGrabAction, PR2GripperGrabGoal, PR2GripperEventDetectorAction, PR2GripperEventDetectorGoal
00054 from trajectory_msgs.msg import JointTrajectoryPoint
00055 from pr2_manipulation_controllers.srv import CheckMoving, MoveToPose, MoveToPoseRequest
00056 from std_srvs.srv import Empty, EmptyRequest
00057 from arm_navigation_msgs.msg import MoveArmAction, MoveArmGoal
00058 from arm_navigation_msgs.msg import JointConstraint, PositionConstraint, OrientationConstraint, \
00059     ArmNavigationErrorCodes, AllowedContactSpecification
00060 from arm_navigation_msgs.msg import Shape
00061 from tabletop_collision_map_processing import collision_map_interface
00062 import numpy
00063 import math
00064 import pdb
00065 import copy
00066 from interpolated_ik_motion_planner import ik_utilities
00067 from object_manipulator.convert_functions import *
00068 import joint_states_listener
00069 from actionlib_msgs.msg import GoalStatus
00070 from tf_conversions import posemath
00071 from pr2_gripper_reactive_approach.controller_params import *
00072 import threading
00073 import scipy.linalg
00074 
00075 ##class to start/stop/switch between joint and Cartesian controllers
00076 class ControllerManager():
00077 
00078   #whicharm is 'r' or 'l'
00079   def __init__(self, whicharm, tf_listener = None, using_slip_controller = 0, using_slip_detection = 0): 
00080     self.whicharm = whicharm
00081 
00082     self.using_slip_controller = using_slip_controller
00083     self.using_slip_detection = using_slip_detection
00084 
00085     rospy.loginfo("initializing "+whicharm+" controller manager")
00086     rospy.loginfo("controller manager: using_slip_controller:"+str(using_slip_controller))
00087     rospy.loginfo("controller manager: using_slip_detection:"+str(using_slip_detection))
00088 
00089     #wait for the load/unload/switch/list controller services to be there and initialize their service proxies
00090     rospy.loginfo("controller manager waiting for pr2_controller_manager services to be there")
00091     load_controller_serv_name = 'pr2_controller_manager/load_controller'
00092     unload_controller_serv_name = 'pr2_controller_manager/unload_controller'
00093     switch_controller_serv_name = 'pr2_controller_manager/switch_controller'
00094     list_controllers_serv_name = 'pr2_controller_manager/list_controllers'
00095     self.wait_for_service(load_controller_serv_name)
00096     self.wait_for_service(unload_controller_serv_name)
00097     self.wait_for_service(switch_controller_serv_name)
00098     self.wait_for_service(list_controllers_serv_name)
00099     self.load_controller_service = \
00100         rospy.ServiceProxy(load_controller_serv_name, LoadController)
00101     self.unload_controller_service = \
00102         rospy.ServiceProxy(unload_controller_serv_name, UnloadController)
00103     self.switch_controller_service = \
00104         rospy.ServiceProxy(switch_controller_serv_name, SwitchController)
00105     self.list_controllers_service = \
00106         rospy.ServiceProxy(list_controllers_serv_name, ListControllers)
00107 
00108     #initialize listener for JointStates messages (separate thread)
00109     self.joint_states_listener = joint_states_listener.LatestJointStates()
00110 
00111     #joint trajectory action client
00112     joint_trajectory_action_name = whicharm+'_arm_controller/joint_trajectory_action'
00113     self.joint_action_client = \
00114         actionlib.SimpleActionClient(joint_trajectory_action_name, JointTrajectoryAction)
00115 
00116     #slip-sensing gripper action client
00117     if self.using_slip_controller:
00118       gripper_action_name = whicharm+'_gripper_sensor_controller/gripper_action'
00119       self.gripper_action_client = actionlib.SimpleActionClient(gripper_action_name, Pr2GripperCommandAction)
00120 
00121     #default gripper action client
00122     else:
00123       gripper_action_name = whicharm+'_gripper_controller/gripper_action'
00124       self.gripper_action_client = \
00125           actionlib.SimpleActionClient(gripper_action_name, Pr2GripperCommandAction)
00126 
00127     #other slip-sensing gripper actions
00128     if self.using_slip_detection:
00129       gripper_find_contact_action_name = whicharm+'_gripper_sensor_controller/find_contact'
00130       gripper_grab_action_name = whicharm+'_gripper_sensor_controller/grab'
00131       gripper_event_detector_action_name = whicharm+'_gripper_sensor_controller/event_detector'
00132 
00133       self.gripper_find_contact_action_client = actionlib.SimpleActionClient(gripper_find_contact_action_name, \
00134                                                                                PR2GripperFindContactAction)
00135       self.gripper_grab_action_client = actionlib.SimpleActionClient(gripper_grab_action_name, \
00136                                                                              PR2GripperGrabAction)
00137       self.gripper_event_detector_action_client = actionlib.SimpleActionClient(gripper_event_detector_action_name, \
00138                                                                              PR2GripperEventDetectorAction)
00139 
00140     #move arm client is filled in the first time it's called
00141     self.move_arm_client = None
00142     
00143     #wait for the joint trajectory and gripper action servers to be there
00144     self.wait_for_action_server(self.joint_action_client, joint_trajectory_action_name)
00145     self.wait_for_action_server(self.gripper_action_client, gripper_action_name)
00146     if self.using_slip_detection:
00147       self.wait_for_action_server(self.gripper_find_contact_action_client, gripper_find_contact_action_name)
00148       self.wait_for_action_server(self.gripper_grab_action_client, gripper_grab_action_name)
00149       self.wait_for_action_server(self.gripper_event_detector_action_client, gripper_event_detector_action_name)
00150 
00151     #initialize a tf listener
00152     if tf_listener == None:
00153       self.tf_listener = tf.TransformListener()
00154     else:
00155       self.tf_listener = tf_listener
00156 
00157     #initialize a collision map interface
00158     self.collision_map_interface = collision_map_interface.CollisionMapInterface()
00159 
00160     #which Cartesian controller to use
00161     self.use_trajectory_cartesian = rospy.get_param('~use_trajectory_cartesian', 0)
00162     self.use_task_cartesian = rospy.get_param('~use_task_cartesian', 0)
00163     if self.use_trajectory_cartesian and self.use_task_cartesian:
00164       rospy.loginfo("can't use both trajectory and task controllers!  Defaulting to task")
00165       self.use_trajectory_cartesian = 0
00166     rospy.loginfo("controller_manager: use_trajectory_cartesian: "+str(self.use_trajectory_cartesian))
00167     rospy.loginfo("controller_manager: use_task_cartesian: "+str(self.use_task_cartesian))
00168 
00169     #names of the controllers
00170     if self.use_trajectory_cartesian:
00171       self.cartesian_controllers = ['_arm_cartesian_pose_controller', 
00172                                     '_arm_cartesian_trajectory_controller']
00173       self.cartesian_controllers = [self.whicharm+x for x in self.cartesian_controllers]
00174     else:
00175       self.cartesian_controllers = [self.whicharm + '_cart']
00176     self.joint_controller = self.whicharm+'_arm_controller'
00177     if self.using_slip_controller:
00178       self.gripper_controller = self.whicharm+'_gripper_sensor_controller'
00179     else:
00180       self.gripper_controller = self.whicharm+'_gripper_controller'
00181 
00182     #parameters for the Cartesian controllers    
00183     if self.use_trajectory_cartesian:
00184       self.cart_params = JTCartesianParams(self.whicharm)
00185     else:
00186       self.cart_params = JTCartesianTaskParams(self.whicharm, self.use_task_cartesian)
00187 
00188     #parameters for the joint controller
00189     self.joint_params = JointParams(self.whicharm)
00190 
00191     #load the Cartesian controllers if not already loaded
00192     rospy.loginfo("loading any unloaded Cartesian controllers")
00193     self.load_cartesian_controllers()
00194     time.sleep(2)
00195     rospy.loginfo("done loading controllers")
00196 
00197     #services for the J-transpose Cartesian controller 
00198     if self.use_trajectory_cartesian:
00199       cartesian_check_moving_name = whicharm+'_arm_cartesian_trajectory_controller/check_moving'
00200       cartesian_move_to_name = whicharm+'_arm_cartesian_trajectory_controller/move_to'
00201       cartesian_preempt_name = whicharm+'_arm_cartesian_trajectory_controller/preempt'
00202       self.wait_for_service(cartesian_check_moving_name)      
00203       self.wait_for_service(cartesian_move_to_name)
00204       self.wait_for_service(cartesian_preempt_name)      
00205       self.cartesian_moving_service = rospy.ServiceProxy(cartesian_check_moving_name, CheckMoving)
00206       self.cartesian_cmd_service = rospy.ServiceProxy(cartesian_move_to_name, MoveToPose)
00207       self.cartesian_preempt_service = rospy.ServiceProxy(cartesian_preempt_name, Empty)
00208 
00209       #re-load the Cartesian controllers with the gentle params
00210       self.cart_params.set_params_to_gentle()
00211       self.reload_cartesian_controllers()
00212     else:
00213       cartesian_cmd_name = whicharm+'_cart/command_pose'
00214       self.cartesian_cmd_pub = rospy.Publisher(cartesian_cmd_name, PoseStamped) 
00215 
00216     #create an IKUtilities class object
00217     rospy.loginfo("creating IKUtilities class objects")
00218     if whicharm == 'r':
00219       self.ik_utilities = ik_utilities.IKUtilities('right', self.tf_listener)
00220     else:
00221       self.ik_utilities = ik_utilities.IKUtilities('left', self.tf_listener)
00222     rospy.loginfo("done creating IKUtilities class objects")
00223 
00224     #joint names for the arm
00225     joint_names = ["_shoulder_pan_joint", 
00226                    "_shoulder_lift_joint", 
00227                    "_upper_arm_roll_joint", 
00228                    "_elbow_flex_joint", 
00229                    "_forearm_roll_joint", 
00230                    "_wrist_flex_joint", 
00231                    "_wrist_roll_joint"]
00232     self.joint_names = [whicharm + x for x in joint_names]
00233 
00234     #thread and params to send clipped/overshoot versions of the last Cartesian command
00235     if not self.use_trajectory_cartesian:
00236       self.dist_tol = .001
00237       self.angle_tol = .05
00238       self.overshoot_dist = 0.005
00239       self.overshoot_angle = 0.087
00240       self.timestep = 1./30.
00241       self.cartesian_command_lock = threading.Lock()
00242       self.cartesian_command_thread_running = False
00243       self.cartesian_desired_pose = self.get_current_wrist_pose_stamped('/base_link')
00244       self.cartesian_command_thread = threading.Thread(target=self.cartesian_command_thread_func)
00245       self.cartesian_command_thread.setDaemon(True)
00246       self.cartesian_command_thread.start()
00247 
00248     rospy.loginfo("done with controller_manager init for the %s arm"%whicharm)
00249 
00250 
00251   ##wait for an action server to be ready
00252   def wait_for_action_server(self, client, name):
00253     while not rospy.is_shutdown():  
00254       rospy.loginfo("controller manager: waiting for %s to be there"%name)
00255       if client.wait_for_server(rospy.Duration(5.0)):
00256         break
00257     rospy.loginfo("controller manager: %s found"%name)  
00258 
00259 
00260   ##wait for a service to be ready
00261   def wait_for_service(self, name):
00262     while not rospy.is_shutdown():  
00263       rospy.loginfo("controller manager: waiting for %s to be there"%name)
00264       try:
00265         rospy.wait_for_service(name, 5.0)
00266       except rospy.ROSException:
00267         continue
00268       break
00269     rospy.loginfo("controller manager: %s found"%name)  
00270 
00271 
00272   ##tell the gripper to close until contact (on one or both finger pads)
00273   #contacts_desired is "both", "left", "right", or "either"
00274   def find_gripper_contact(self, contacts_desired, zero_fingertips = 0, blocking = 1, timeout = 12.):
00275 
00276     goal = PR2GripperFindContactGoal()
00277     contacts_desired_dict = {"both":goal.command.BOTH, "left":goal.command.LEFT, "right":goal.command.RIGHT, "either":goal.command.EITHER}
00278 
00279     goal.command.contact_conditions = contacts_desired_dict[contacts_desired]
00280     goal.command.zero_fingertip_sensors = zero_fingertips
00281     
00282     rospy.loginfo("controller manager: sending find contact goal")
00283     self.gripper_find_contact_action_client.send_goal(goal)
00284     
00285     #if blocking is requested, wait for the state to reach the desired state
00286     if blocking:
00287       finished_within_time = self.gripper_find_contact_action_client.wait_for_result(rospy.Duration(timeout))
00288       if not finished_within_time:
00289         self.gripper_find_contact_action_client.cancel_goal()
00290         rospy.logerr("Gripper didn't see the desired number of contacts while closing in time")
00291         return (0, 0, 0, 0)
00292       state = self.gripper_find_contact_action_client.get_state()
00293       if state == GoalStatus.SUCCEEDED:
00294         result = self.gripper_find_contact_action_client.get_result()
00295         return (result.data.left_fingertip_pad_contact, result.data.right_fingertip_pad_contact, \
00296                   result.data.left_fingertip_pad_contact_force, result.data.right_fingertip_pad_contact_force)
00297       return (0,0,0,0)
00298 
00299 
00300   ##get the state from find_gripper_contact 
00301   def get_find_gripper_contact_state(self):
00302     return self.gripper_find_contact_action_client.get_state() 
00303 
00304 
00305   ##get the final result from find_gripper_contact
00306   def get_find_gripper_contact_result(self):
00307     result = self.gripper_find_contact_action_client.get_result()
00308     if result:
00309       return (result.data.left_fingertip_pad_contact, result.data.right_fingertip_pad_contact, \
00310                 result.data.left_fingertip_pad_force, result.data.right_fingertip_pad_force)
00311     else:
00312       rospy.logerr("gripper_find_contact_action_client.get_result returned None!")
00313       return (0, 0, 0, 0)
00314 
00315 
00316   ##opens and closes the gripper to determine how hard to grasp the object, then starts up slip servo mode
00317   def start_gripper_grab(self,  hardness_gain = 0.035, timeout = 10., blocking = 1):
00318     
00319     goal = PR2GripperGrabGoal()
00320     goal.command.hardness_gain = hardness_gain
00321 
00322     rospy.loginfo("starting slip controller grab")
00323     self.gripper_grab_action_client.send_goal(goal)
00324 
00325     if blocking:
00326       finished_within_time = self.gripper_grab_action_client.wait_for_result(rospy.Duration(timeout))
00327       if not finished_within_time:
00328         self.gripper_find_contact_action_client.cancel_goal()
00329         rospy.logerr("Gripper grab timed out")
00330       state = self.gripper_find_contact_action_client.get_state()
00331       if state == GoalStatus.SUCCEEDED:
00332         return 1
00333       return 0
00334 
00335     return 1
00336 
00337 
00338   ##start up gripper event detector to detect when an object hits the table 
00339   #or when someone is trying to take an object from the robot
00340   def start_gripper_event_detector(self, blocking = 0, timeout = 15.):
00341     
00342     goal = PR2GripperEventDetectorGoal()
00343     goal.command.trigger_conditions = goal.command.FINGER_SIDE_IMPACT_OR_SLIP_OR_ACC  #use either slip or acceleration as a contact condition
00344     goal.command.acceleration_trigger_magnitude = 3.25  #contact acceleration used to trigger 
00345     goal.command.slip_trigger_magnitude = 0.008        #contact slip used to trigger    
00346 
00347     rospy.loginfo("starting gripper event detector")
00348     self.gripper_event_detector_action_client.send_goal(goal)
00349 
00350     #if blocking is requested, wait until the action returns
00351     if blocking:
00352       finished_within_time = self.gripper_event_detector_action_client.wait_for_result(rospy.Duration(timeout))
00353       if not finished_within_time:
00354         rospy.logerr("Gripper didn't see the desired event trigger before timing out")
00355         return 0
00356       state = self.gripper_event_detector_action_client.get_state()
00357       if state == GoalStatus.SUCCEEDED:
00358         result = self.gripper_event_detector_action_client.get_result()
00359         if result.data.placed:
00360           return 1
00361       return 0
00362 
00363 
00364   ##get the state from the gripper event detector
00365   def get_gripper_event_detector_state(self):
00366     return self.gripper_event_detector_action_client.get_state()
00367 
00368 
00369   ##create a basic goal message for move_arm
00370   def create_move_arm_goal(self):
00371     goal = MoveArmGoal()
00372     if self.whicharm == "r":
00373       goal.motion_plan_request.group_name = "right_arm"
00374     else:
00375       goal.motion_plan_request.group_name = "left_arm"
00376     goal.motion_plan_request.num_planning_attempts = 2;
00377     goal.motion_plan_request.allowed_planning_time = rospy.Duration(5.0);
00378     goal.motion_plan_request.planner_id = ""
00379     goal.planner_service_name = "ompl_planning/plan_kinematic_path"
00380     return goal
00381 
00382 
00383   ##send a goal to move arm and wait for the result, modifying the goal if the start state is in collision
00384   #returns 0 if timed out; otherwise an ArmNavigationErrorCodes value (int32, 1=success) from move_arm 
00385   def send_move_arm_goal(self, goal, blocking = 1):
00386 
00387     #create an action client, if this is the first call to move_arm
00388     if self.move_arm_client == None:
00389       if self.whicharm == "r":
00390         self.move_arm_client = actionlib.SimpleActionClient('move_right_arm', MoveArmAction)
00391       if self.whicharm == "l":
00392         self.move_arm_client = actionlib.SimpleActionClient('move_left_arm', MoveArmAction)
00393       rospy.loginfo("waiting for move arm server")
00394       self.move_arm_client.wait_for_server()
00395       rospy.loginfo("move arm server was there")
00396 
00397     #send the goal off
00398     self.move_arm_client.send_goal(goal)
00399 
00400     #wait for the move to finish and return the result
00401     if blocking:
00402       for add_contact_tries in range(10):
00403         finished_within_time = self.move_arm_client.wait_for_result(rospy.Duration(60))
00404         if not finished_within_time:
00405           self.move_arm_client.cancel_goal()
00406           rospy.logerr("Timed out when asking move_arm to go to a joint goal")
00407           return 0
00408 
00409         state = self.move_arm_client.get_state()
00410         result = self.move_arm_client.get_result()
00411         if state == GoalStatus.SUCCEEDED:
00412           if result.error_code.val == 1:
00413             rospy.loginfo("move_arm succeeded")
00414             break
00415 
00416           #start state was in collision!  Try to add contact regions to ignore the start collisions
00417           elif result.error_code.val == ArmNavigationErrorCodes.START_STATE_IN_COLLISION:
00418             #rospy.loginfo("move arm start state in collision!  Adding allowed contact regions and trying again, try number %d"%add_contact_tries)
00419             #self.modify_move_arm_goal(goal, result.contacts)
00420             rospy.loginfo("move arm start state in collision!  Resetting the collision map and trying again, try number %d"%add_contact_tries)
00421             repopulate = True
00422             if add_contact_tries > 2:
00423               rospy.logerr("Not repopulating collision map, too many tries!")
00424               repopulate = False
00425             self.reset_collision_map(False, repopulate)
00426             self.move_arm_client.send_goal(goal)
00427             continue
00428         else:
00429           rospy.logerr("move_arm did not succeed, state %d, error code %d"%(state, result.error_code.val))
00430           break
00431       return result.error_code.val
00432     else:
00433       return 1
00434 
00435 
00436   ##reset the collision map (and optionally all attached/collision objects)
00437   def reset_collision_map(self, clear_objects, repopulate):
00438     if clear_objects:
00439       self.collision_map_interface.reset_collision_map()
00440     else:
00441       self.collision_map_interface.clear_octomap()
00442     if repopulate:
00443       rospy.sleep(5.0)
00444 
00445 
00446   ##set the planning scene (for IK/move_arm calls)
00447   def set_planning_scene(self):
00448     self.collision_map_interface.set_planning_scene()
00449 
00450 
00451   ##use move_arm to get to a desired joint configuration in a collision-free way
00452   #returns 0 if timed out; otherwise an ArmNavigationErrorCodes (int32, 1=success) from move_arm 
00453   def move_arm_joint(self, joint_angles, blocking = 1):
00454     self.check_controllers_ok('joint')
00455     self.set_planning_scene()
00456 
00457     rospy.loginfo("asking move_arm to go to angles: %s"%self.pplist(joint_angles))
00458 
00459     goal = self.create_move_arm_goal()
00460     #goal.disable_collision_monitoring = 1
00461 
00462     #add the joint angles as a joint constraint
00463     for (joint_name, joint_angle) in zip(self.joint_names, joint_angles):
00464       joint_constraint = JointConstraint()
00465       joint_constraint.joint_name = joint_name
00466       joint_constraint.position = joint_angle
00467       joint_constraint.tolerance_below = .1
00468       joint_constraint.tolerance_above = .1
00469       goal.motion_plan_request.goal_constraints.joint_constraints.append(joint_constraint)
00470 
00471     #send the goal off to move arm, blocking if desired and modifying the start state if it's in collision
00472     result = self.send_move_arm_goal(goal, blocking)
00473     return result
00474 
00475 
00476   ##if move_arm_joint returns an error code of START_STATE_IN_COLLISION, we can try to add 
00477   #allowed contact regions to get it out of the start collisions (appears to already be done in C++)
00478   def modify_move_arm_goal(self, goal, contact_info):
00479     allowed_contacts = []
00480     allowed_penetration_depth = .5
00481     for (i, contact) in enumerate(contact_info):
00482       allowed_contact_tmp = AllowedContactSpecification()
00483       allowed_contact_tmp.shape.type = allowed_contact_tmp.shape.BOX
00484       allowed_contact_tmp.shape.dimensions = [allowed_penetration_depth]*3
00485       allowed_contact_tmp.pose_stamped.pose.position = contact.position
00486       set_xyzw(allowed_contact_tmp.pose_stamped.pose.orientation, [0,0,0,1])
00487       allowed_contact_tmp.pose_stamped.header.stamp = rospy.Time.now()
00488       allowed_contact_tmp.pose_stamped.header.frame_id = contact.header.frame_id
00489       allowed_contact_tmp.link_names.append(contact.contact_body_1)
00490       allowed_contact_tmp.penetration_depth = allowed_penetration_depth
00491       allowed_contacts.append(allowed_contact_tmp);
00492       rospy.loginfo("Added allowed contact region: %d"%i)
00493       #rospy.loginfo("Position                    : (%f,%f,%f)"%(contact.position.x, \
00494       #         contact.position.y,contact.position.z))
00495       #rospy.loginfo("Frame id                    : %s"%contact.header.frame_id);
00496       rospy.loginfo("Bodies                      : %s and %s"%(contact.contact_body_1, contact.contact_body_2))
00497     goal.motion_plan_request.allowed_contacts.extend(allowed_contacts)
00498 
00499 
00500   ##move the wrist to the desired location while keeping the current wrist orientation upright
00501   #start angles are the joint angle start point for IK searches
00502   #default start angles and location are for moving the arm to the side
00503   #returns 0 if timed out or no IK solution; otherwise an ArmNavigationErrorCodes (int32, 1=success) from move_arm 
00504   def move_arm_constrained(self, constraint_in = None, start_angles = None, location = None, blocking = 1, compute_feasible_goal = True, frame_id = 'torso_lift_link'):
00505 
00506     #set the planning scene
00507     self.set_planning_scene()
00508 
00509     self.check_controllers_ok('joint')
00510     current_pose = self.get_current_wrist_pose_stamped(frame_id)
00511     move_arm_goal = self.create_move_arm_goal()
00512     if move_arm_goal.motion_plan_request.group_name == "right_arm":
00513       move_arm_goal.motion_plan_request.group_name = "right_arm_cartesian"
00514     if move_arm_goal.motion_plan_request.group_name == "left_arm":
00515       move_arm_goal.motion_plan_request.group_name = "left_arm_cartesian"
00516  
00517     #add a position constraint for the goal
00518     position_constraint = PositionConstraint()
00519     position_constraint.header.frame_id = frame_id
00520     position_constraint.link_name = self.ik_utilities.link_name #r_ or l_wrist_roll_link
00521     set_xyz(position_constraint.position, location)
00522     position_constraint.constraint_region_shape.type = Shape.BOX
00523     position_constraint.constraint_region_shape.dimensions = [0.02]*3
00524     position_constraint.constraint_region_orientation.w = 1.0
00525     position_constraint.weight = 1.0
00526     move_arm_goal.motion_plan_request.goal_constraints.position_constraints.append(position_constraint)
00527 
00528     orientation_constraint = copy.deepcopy(constraint_in.orientation_constraints[0])
00529 
00530     #search for a feasible goal pose that gets our wrist to location while keeping all but the (base-link) yaw fixed
00531     #TODO modify this so it can deal with general constraints
00532     if compute_feasible_goal == True:
00533       current_pose_mat = pose_to_mat(current_pose.pose)
00534       found_ik_solution = 0
00535       for angle in range(0, 180, 5):
00536         for dir in [1, -1]:
00537           rot_mat = tf.transformations.rotation_matrix(dir*angle/180.*math.pi, [0,0,1])
00538           rotated_pose_mat = rot_mat * current_pose_mat
00539           desired_pose = stamp_pose(mat_to_pose(rotated_pose_mat), frame_id)
00540           desired_pose.pose.position = position_constraint.position
00541 
00542           #check if there's a collision-free IK solution at the goal location with that orientation
00543           (solution, error_code) = self.ik_utilities.run_ik(desired_pose, start_angles, \
00544                                                     self.ik_utilities.link_name, collision_aware = 1)
00545           if error_code == "SUCCESS":
00546             found_ik_solution = 1
00547 
00548             rospy.loginfo("IK solution found for constrained goal")
00549             orientation_constraint.orientation = desired_pose.pose.orientation 
00550             break
00551 
00552         if found_ik_solution:
00553           break
00554       else:
00555         rospy.loginfo("no IK solution found for goal, aborting")
00556         return 0
00557 
00558     move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints.append(orientation_constraint)
00559 
00560     #add a joint constraint for the upper arm roll joint corresponding to our found solution
00561     joint_constraint = JointConstraint()
00562     if move_arm_goal.motion_plan_request.group_name == "right_arm_cartesian":
00563       joint_constraint.joint_name = "r_upper_arm_roll_joint"
00564     if move_arm_goal.motion_plan_request.group_name == "left_arm_cartesian":
00565       joint_constraint.joint_name = "l_upper_arm_roll_joint"
00566     joint_constraint.position = solution[2]
00567     joint_constraint.tolerance_below = .1
00568     joint_constraint.tolerance_above = .1
00569     move_arm_goal.motion_plan_request.goal_constraints.joint_constraints.append(joint_constraint)
00570 
00571     #add an orientation constraint for the entire path to keep the object upright
00572     path_orientation_constraint = copy.deepcopy(orientation_constraint)
00573     
00574     #temporary, while deepcopy of rospy.Time is broken
00575     path_orientation_constraint.header.stamp = rospy.Time(orientation_constraint.header.stamp.secs)
00576     path_orientation_constraint.orientation = current_pose.pose.orientation
00577     move_arm_goal.motion_plan_request.path_constraints.orientation_constraints.append(path_orientation_constraint)    
00578   
00579     move_arm_goal.motion_plan_request.planner_id = "SBLkConfig1"
00580     move_arm_goal.disable_ik = True
00581     move_arm_goal.planner_service_name= "/ompl_planning/plan_kinematic_path/"
00582 
00583     #send the goal off to move arm, blocking if desired and modifying the start state if it's in collision
00584     result = self.send_move_arm_goal(move_arm_goal, blocking)
00585     return result
00586 
00587 
00588   ##use move_arm to get to a desired pose in a collision-free way (really, run IK and then call move arm to move to the IK joint angles)
00589   #returns 0 if timed out or no IK solution; otherwise an ArmNavigationErrorCodes (int32, 1=success) from move_arm 
00590   def move_arm_pose(self, pose_stamped, start_angles = None, blocking = 1):
00591 
00592     #set the planning scene
00593     self.set_planning_scene()
00594 
00595     if start_angles == None:
00596       start_angles = self.get_current_arm_angles()
00597     (solution, error_code) = self.ik_utilities.run_ik(pose_stamped, start_angles, \
00598                     self.ik_utilities.link_name)
00599     if not solution:
00600       rospy.logerr("no IK solution found for goal pose!")
00601       return 0
00602     else:
00603       result = self.move_arm_joint(solution, blocking)
00604       return result
00605 
00606 
00607   ##normalize a trajectory (list of lists of joint angles), so that the desired angles 
00608   #are the nearest ones for the continuous joints (5 and 7)
00609   def normalize_trajectory(self, trajectory):
00610     current_angles = self.get_current_arm_angles()
00611     trajectory_copy = [list(angles) for angles in trajectory]
00612     for angles in trajectory_copy:
00613       angles[4] = self.normalize_angle(angles[4], current_angles[4])
00614       angles[6] = self.normalize_angle(angles[6], current_angles[6])
00615     return trajectory_copy
00616 
00617 
00618   ##normalize an angle for a continuous joint so that it's the closest version 
00619   #of the angle to the current angle (not +-2*pi)
00620   def normalize_angle(self, angle, current_angle):
00621     while current_angle-angle > math.pi:
00622       angle += 2*math.pi
00623     while angle - current_angle > math.pi:
00624       angle -= 2*math.pi
00625     return angle
00626 
00627 
00628   ##find and execute a joint-angle trajectory to perform a collision-free Cartesian movement
00629   #if start_pose is None, starts from the current pose (if not None, will go straight to the start pose using the joint trajectory action)
00630   def command_interpolated_ik(self, end_pose, start_pose = None, collision_aware = 1, step_size = .02, max_joint_vel = .05):
00631     self.check_controllers_ok('joint')
00632 
00633     #find the current joint angles 
00634     current_angles = self.get_current_arm_angles()
00635 
00636     if start_pose == None:
00637       #use the current wrist pose as the start pose/angles
00638       (current_trans, current_rot) = self.return_cartesian_pose()
00639 
00640       #put the current pose into a poseStamped
00641       start_pose = create_pose_stamped(current_trans+current_rot)
00642       #print "start_pose:\n", start_pose
00643     #print "end_pose:\n", end_pose
00644 
00645     #set the planning scene
00646     self.set_planning_scene()
00647 
00648     #find a collision-free joint trajectory to move from the current pose 
00649     #to the desired pose using Cartesian interpolation
00650     (trajectory, error_codes) = self.ik_utilities.check_cartesian_path(start_pose, \
00651                      end_pose, current_angles, step_size, .1, math.pi/4, collision_aware)
00652 
00653     #if some point is not possible, quit
00654     if any(error_codes):
00655       rospy.loginfo("can't execute an interpolated IK trajectory to that pose")
00656       return 0
00657 
00658 #     print "found trajectory:"
00659 #     for point in trajectory:
00660 #       if type(point) == tuple:
00661 #         print self.pplist(point)
00662 #       else:
00663 #         print point
00664 
00665     #send the trajectory to the joint trajectory action
00666     #print "moving through trajectory"
00667     self.command_joint_trajectory(trajectory, max_joint_vel)
00668     return 1
00669 
00670 
00671   ##use the joint_states_listener to get the arm angles
00672   def get_current_arm_angles(self):
00673     (found, positions, vels, efforts) = \
00674         self.joint_states_listener.return_joint_states(self.joint_names)
00675     return positions
00676 
00677 
00678   ##return the current pose of the wrist as a PoseStamped
00679   def get_current_wrist_pose_stamped(self, frame = 'base_link'):
00680       (current_trans, current_rot) = self.return_cartesian_pose(frame)
00681       return create_pose_stamped(current_trans+current_rot, frame)
00682 
00683 
00684   ##use the joint_states_listener to get the current gripper opening
00685   def get_current_gripper_opening(self):
00686     (found, positions, vels, efforts) = \
00687         self.joint_states_listener.return_joint_states([self.whicharm+'_gripper_joint'])
00688     return positions[0]
00689 
00690 
00691   ##clip pose to be no more than max_pos_dist or max_rot_dist away from the current gripper pose
00692   def clip_pose(self, pose, max_pos_dist = 0.02, max_rot_dist = math.pi/20., current_pose = None):
00693     if current_pose == None:
00694       current_pose = self.get_current_wrist_pose_stamped(pose.header.frame_id)
00695     current_mat = pose_to_mat(current_pose.pose)
00696     target_mat = pose_to_mat(pose.pose)
00697 
00698     target_to_current = current_mat**-1 * target_mat
00699     desired_trans = target_to_current[0:3, 3].copy()
00700     desired_trans_mag = scipy.linalg.norm(desired_trans)
00701     target_to_current[0:3, 3] = 0
00702 
00703     desired_angle, desired_axis, point = tf.transformations.rotation_from_matrix(target_to_current)
00704             
00705     frac_trans = math.fabs(desired_trans_mag / max_pos_dist)
00706     frac_rot = math.fabs(desired_angle / max_rot_dist)
00707 
00708     if frac_trans <= 1 and frac_rot <= 1:
00709       return pose
00710     frac = max(frac_rot, frac_trans)
00711 
00712     clamped_angle = desired_angle / frac
00713     clamped_trans = desired_trans / frac
00714 
00715     clamped_transformation = scipy.matrix(tf.transformations.rotation_matrix(clamped_angle, desired_axis))
00716     clamped_transformation[0:3, 3] = clamped_trans
00717     clamped_mat = current_mat * clamped_transformation
00718     clamped_pose = stamp_pose(mat_to_pose(clamped_mat), pose.header.frame_id)
00719     return clamped_pose
00720 
00721   
00722   ##overshoot pose by overshoot_dist and overshoot_angle, unless we're already within dist_tol/angle_tol
00723   def overshoot_pose(self, desired_pose, overshoot_dist, overshoot_angle, dist_tol, angle_tol, current_pose = None):
00724     if current_pose == None:
00725       current_pose = self.get_current_wrist_pose_stamped(desired_pose.header.frame_id)
00726     current_mat = pose_to_mat(current_pose.pose)
00727     target_mat = pose_to_mat(desired_pose.pose)
00728 
00729     target_to_current = current_mat**-1 * target_mat
00730     desired_trans = target_to_current[0:3, 3].copy()
00731     desired_trans_mag = scipy.linalg.norm(desired_trans)
00732     target_to_current[0:3, 3] = 0
00733 
00734     desired_angle, desired_axis, point = tf.transformations.rotation_from_matrix(target_to_current)
00735 
00736     if desired_trans_mag <= dist_tol and abs(desired_angle) <= angle_tol:
00737       return desired_pose
00738     pos_change = 0
00739     angle_change = 0
00740     if desired_trans_mag > dist_tol:
00741       pos_change = (desired_trans_mag + overshoot_dist)/desired_trans_mag * desired_trans
00742     if abs(desired_angle) > angle_tol:
00743       angle_sign = desired_angle / abs(desired_angle)
00744       angle_change = desired_angle + angle_sign*overshoot_angle
00745     
00746     overshoot_transformation = scipy.matrix(tf.transformations.rotation_matrix(angle_change, desired_axis))
00747     overshoot_transformation[0:3, 3] = pos_change
00748     overshoot_mat = current_mat * overshoot_transformation
00749     overshoot_pose = stamp_pose(mat_to_pose(overshoot_mat), desired_pose.header.frame_id)
00750     return overshoot_pose
00751 
00752 
00753   ##thread function for sending clipped Cartesian commands
00754   def cartesian_command_thread_func(self):
00755     r = rospy.Rate(1./self.timestep)
00756     while not rospy.is_shutdown():
00757       with self.cartesian_command_lock:
00758         running = self.cartesian_command_thread_running
00759       if running:
00760         self.cartesian_desired_pose.header.stamp = rospy.Time.now()
00761         with self.cartesian_command_lock:
00762           desired_pose = self.cartesian_desired_pose
00763           overshoot_dist = self.overshoot_dist
00764           overshoot_angle = self.overshoot_angle
00765           dist_tol = self.dist_tol
00766           angle_tol = self.angle_tol
00767 
00768         current_pose = self.get_current_wrist_pose_stamped(desired_pose.header.frame_id)
00769 
00770         #compute a new desired pose with the desired overshoot
00771         overshoot_pose = self.overshoot_pose(desired_pose, overshoot_dist, overshoot_angle, 
00772                                                    dist_tol, angle_tol, current_pose)
00773 
00774         #clip the pose if it's too far away
00775         clipped_pose = self.clip_pose(overshoot_pose, current_pose = current_pose)
00776         #clipped_pose = self.clip_pose(desired_pose, current_pose = current_pose)
00777 
00778         self.cartesian_cmd_pub.publish(clipped_pose)
00779       r.sleep()
00780       
00781 
00782   ##stop sending commands from the Cartesian command thread
00783   def stop_cartesian_commands(self):
00784     if not self.use_trajectory_cartesian:
00785       with self.cartesian_command_lock:
00786         self.cartesian_command_thread_running = False
00787 
00788 
00789   ##start sending commands from the Cartesian command thread
00790   def start_cartesian_commands(self):
00791     if not self.use_trajectory_cartesian:
00792       with self.cartesian_command_lock:
00793         self.cartesian_command_thread_running = True
00794 
00795 
00796   ##set the current desired Cartesian pose to the current gripper pose
00797   def set_desired_cartesian_to_current(self):
00798     if not self.use_trajectory_cartesian:
00799       current_pose = self.get_current_wrist_pose_stamped('/base_link')
00800       with self.cartesian_command_lock:
00801         self.cartesian_desired_pose = current_pose
00802 
00803 
00804   ##send a single desired pose to the Cartesian controller 
00805   #(pose is either a PoseStamped or a 7-list of position and quaternion orientation; if the latter, put the frame
00806   #in frame_id) 
00807   def command_cartesian(self, pose, frame_id = '/base_link'):
00808     self.check_controllers_ok('cartesian')
00809 
00810     if type(pose) == list:
00811       m = create_pose_stamped(pose, frame_id)
00812     else:
00813       m = pose
00814 
00815     if self.use_trajectory_cartesian:
00816       req = MoveToPoseRequest()
00817       req.pose = m
00818       try:
00819         self.cartesian_preempt_service(EmptyRequest())
00820       except:
00821         rospy.loginfo("preempt unnecessary, robot not moving")
00822       else:
00823         rospy.loginfo("Cartesian movement preempted before issuing new command")
00824       
00825       self.cartesian_cmd_service(req)
00826 
00827     else:
00828       self.start_cartesian_commands()
00829       with self.cartesian_command_lock:
00830         self.cartesian_desired_pose = m
00831       
00832 
00833   ##check if the Cartesian controller thinks it's gotten to its goal (it's pretty loose about goals)
00834   def check_cartesian_done(self):
00835     if self.use_trajectory_cartesian:
00836       resp = self.cartesian_moving_service()
00837       return resp.ismoving
00838     else:
00839       return 1
00840 
00841 
00842   ##check if we're near to a desired Cartesian pose (either PoseStamped or 7-list of position and quaternion
00843   #in 'base_link' frame) by pos_thres(m) and rot_thres (rad)
00844   def check_cartesian_near_pose(self, goal_pose, pos_thres, rot_thres):
00845     if type(goal_pose) == list:
00846       goal_pos = goal_pose[0:3]
00847       goal_rot = goal_pose[3:7]
00848     else:
00849       (goal_pos, goal_rot) = pose_stamped_to_lists(self.tf_listener, goal_pose, 'base_link')
00850     
00851     (pos_dist, rot_dist) = self.dist_from_current_pos_and_rot(goal_pos, goal_rot)
00852     if pos_dist < pos_thres and rot_dist < rot_thres:
00853       return 1
00854     return 0    
00855 
00856 
00857   ##return the distance and angle from the current Cartesian pose to a goal pose
00858   def dist_from_current_pos_and_rot(self, goal_pos, goal_rot, current_pos = None, current_rot = None):
00859 
00860     if current_pos == None or current_rot == None:
00861       (current_pos, current_rot) = self.return_cartesian_pose()
00862     
00863     #compute how far the wrist is translated from the goal
00864     diff = [x-y for (x,y) in zip(goal_pos, current_pos)]
00865     pos_diff = self.ik_utilities.vect_norm(diff)
00866 
00867     #compute how far the wrist is rotated from the goal
00868     norm_goal_rot = self.ik_utilities.normalize_vect(goal_rot)
00869     norm_current_rot = self.ik_utilities.normalize_vect(current_rot)
00870     rot_diff = self.ik_utilities.quat_angle(norm_current_rot, norm_goal_rot)
00871     #rospy.loginfo("pos_diff: %5.3f, rot_diff: %5.3f"%(pos_diff, rot_diff))
00872 
00873     return (pos_diff, rot_diff)
00874 
00875 
00876   ##check if both the Cartesian controllers think we're done and if we're close to where we want to be
00877   def check_cartesian_really_done(self, goal_pose, pos_thres, rot_thres, current_pos = None, current_rot = None):
00878     if not self.check_cartesian_done():
00879       return 0
00880     if current_pos == None or current_rot == None:
00881       (current_pos, current_rot) = self.return_cartesian_pose()
00882     done = self.check_cartesian_near_pose(goal_pose, pos_thres, rot_thres)
00883     if done:
00884         self.stop_cartesian_commands()
00885     return done
00886 
00887 
00888   ##wait for either the Cartesian pose to get near enough to a goal pose or timeout (a ROS duration) is reached
00889   #or settling_time (a ROS duration) after the Cartesian controllers report that they're done
00890   def wait_cartesian_really_done(self, goal_pose, pos_thres = .02, rot_thres = .1, 
00891                                  timeout = rospy.Duration(30.), settling_time = rospy.Duration(3.0),
00892                                  overshoot_dist = 0., overshoot_angle = 0.,
00893                                  stuck_dist = .03, stuck_angle = .12):
00894 
00895     #set the overshoot and tolerance params for the thread function
00896     if not self.use_trajectory_cartesian:
00897       with self.cartesian_command_lock:
00898         self.dist_tol = pos_thres
00899         self.angle_tol = rot_thres
00900         self.overshoot_dist = overshoot_dist
00901         self.overshoot_angle = overshoot_angle
00902 
00903     #check every so often until we're done
00904     start_time = rospy.get_rostime()
00905     done_time = None
00906     r = rospy.Rate(10)
00907     (last_pos, last_rot) = self.return_cartesian_pose()
00908     last_pos_array = [last_pos]*5
00909     last_rot_array = [last_rot]*5
00910     while not rospy.is_shutdown():
00911       now = rospy.get_rostime()
00912 
00913       #check if we got to the goal
00914       (current_pos, current_rot) = self.return_cartesian_pose()
00915       if self.check_cartesian_really_done(goal_pose, pos_thres, rot_thres, current_pos, current_rot):
00916         rospy.loginfo("got to the goal")        
00917         return "success"
00918 
00919       #check if we're stuck
00920       if stuck_dist or stuck_angle:
00921         (pos_change, angle_change) = self.dist_from_current_pos_and_rot(last_pos_array[0], last_rot_array[0])
00922         for ind in range(len(last_pos_array)-1):
00923           last_pos_array[ind] = last_pos_array[ind+1]
00924           last_rot_array[ind] = last_rot_array[ind+1]
00925         last_pos_array[-1] = current_pos
00926         last_rot_array[-1] = current_rot
00927         if now - start_time > rospy.Duration(2.0) and \
00928               abs(pos_change) <= stuck_dist*self.timestep and \
00929               abs(angle_change) <= stuck_angle*self.timestep:
00930           rospy.loginfo("Cartesian controller is stuck, stopping!")
00931           #rospy.loginfo("pos_change: %.3f, angle_change: %.3f, stuck_dist*self.timestep:%.3f, stuck_angle*self.timestep:%.3f"%(pos_change, angle_change, stuck_dist*self.timestep, stuck_angle*self.timestep))
00932           self.stop_cartesian_commands()
00933           return "failed"
00934 
00935       #if using the trajectory controller, let it settle after it thinks it's done
00936       if done_time == None and self.check_cartesian_done():
00937         #rospy.loginfo("Cartesian controllers think they're done")
00938         done_time = rospy.get_rostime()
00939       if self.use_trajectory_cartesian and done_time and rospy.get_rostime() - done_time > settling_time:
00940         rospy.loginfo("settling time ran out")
00941         return "failed"
00942 
00943       #check if we've timed out
00944       if timeout != 0. and rospy.get_rostime() - start_time > timeout:
00945         rospy.loginfo("timed out")
00946         self.stop_cartesian_commands()
00947         return "timed out"
00948       r.sleep()
00949 
00950 
00951   ##move in Cartesian space using the Cartesian controllers
00952   #!!! if using non-blocking mode, and not separately using wait_cartesian_really_done or 
00953   #check_cartesian_really_done, you must call stop_cartesian_commands yourself when the move 
00954   #is finished or it will keep sending Cartesian commands !!!
00955   #if collision_aware, checks whether the path could be collision-free, but
00956   #uses the Cartesian controllers to execute it
00957   #if blocking, waits for completion, otherwise returns after sending the goal
00958   #step_size only used if collision aware (size of steps to check for collisions)
00959   #pos_thres and rot_thres are thresholds within which the goal is declared reached
00960   #times out and quits after timeout, and waits settling_time after the 
00961   #controllers declare that they're done (which for the trajectory controller, is usually way too early)
00962   #if using the non-trajectory controller, overshoot_dist is how far to overshoot the goal to make 
00963   #sure we get there (it will stop when we actually get there)
00964   #and overshoot_angle is how far to overshoot the goal angle
00965   def move_cartesian(self, goal_pose, collision_aware = 0, blocking = 1,
00966                      step_size = .005, pos_thres = .001, rot_thres = .05,
00967                      timeout = rospy.Duration(30.), 
00968                      settling_time = rospy.Duration(1.),
00969                      overshoot_dist = 0.005, overshoot_angle = 0.05,
00970                      stuck_dist = .005, stuck_angle = .001):
00971 
00972     #get the current wrist pose as the start pose
00973     (current_pos, current_rot) = self.return_cartesian_pose('base_link')
00974     start_pose = create_pose_stamped(current_pos+current_rot)
00975 
00976     #check to see that the path could possibly be collision-free
00977     if collision_aware:
00978       self.set_planning_scene()
00979       current_angles = self.get_current_arm_angles()
00980       (traj, error_codes) = self.ik_utilities.check_cartesian_path(start_pose, \
00981                        goal_pose, current_angles, .01, .1, math.pi/4, 1) 
00982       if any(error_codes):
00983         return "no solution"
00984 
00985     #go to the goal pose using the Cartesian controllers
00986     self.command_cartesian(goal_pose)
00987     if not blocking:
00988       return "sent goal"
00989 
00990     #blocking: wait for the Cartesian controllers to get there or time out
00991     result = self.wait_cartesian_really_done(goal_pose, pos_thres, \
00992                  rot_thres, timeout = timeout, settling_time = settling_time, \
00993                  overshoot_dist = overshoot_dist, overshoot_angle = overshoot_angle, \
00994                  stuck_dist = stuck_dist, stuck_angle = stuck_angle)
00995 
00996 
00997   ##move in Cartesian space using IK Cartesian interpolation
00998   #if collision_aware, quits if the path is not collision-free
00999   #if blocking, waits for completion, otherwise returns after sending the goal
01000   #step_size is the step size for interpolation
01001   #pos_thres and rot_thres are thresholds within which the goal is declared reached
01002   #times out and quits after timeout, and waits settling_time after the 
01003   #controllers declare that they're done
01004   def move_cartesian_ik(self, goal_pose, collision_aware = 0, blocking = 1,
01005                      step_size = .005, pos_thres = .02, rot_thres = .1,
01006                      timeout = rospy.Duration(30.), 
01007                      settling_time = rospy.Duration(0.25), vel = .15):
01008 
01009     #send the interpolated IK goal to the joint trajectory action
01010     result = self.command_interpolated_ik(goal_pose, collision_aware = collision_aware, \
01011                                             step_size = step_size, max_joint_vel = vel)
01012     if not result:
01013       return "no solution"
01014     if not blocking:
01015       return "sent goal"
01016 
01017     #blocking: wait for the joint trajectory action to get there or time out
01018     joint_action_result = self.wait_joint_trajectory_done(timeout)
01019     if joint_action_result == "timed out":
01020       return "timed out"
01021     
01022     if self.check_cartesian_really_done(goal_pose, pos_thres, rot_thres):
01023       return "success"
01024     return "failed"
01025 
01026 
01027   ##make sure the appropriate controllers (joint or Cartesian) are running/stopped
01028   #mode is 'joint' or 'cartesian'
01029   def check_controllers_ok(self, mode):
01030     modewrong = 0
01031     if mode == 'joint':
01032       self.check_controller_states()
01033       if not self.joint_controller_state == 'running' and \
01034             any([x == 'running' for x in self.cartesian_controllers_state]):
01035         self.switch_to_joint_mode()
01036       elif not self.joint_controller_state == 'running':
01037         rospy.logwarn("joint controllers aren't running!  Attempting to start")
01038         self.start_joint_controllers()
01039       elif any([x == 'running' for x in self.cartesian_controllers_state]):
01040         rospy.logwarn("Cartesian controllers are running!  Attempting to stop")
01041         self.stop_controllers(stop_cartesian = 1)
01042       self.stop_cartesian_commands()
01043 
01044     elif mode == 'cartesian':
01045       self.check_controller_states()
01046       if not all([x == 'running' for x in self.cartesian_controllers_state]) and \
01047             self.joint_controller_state == 'running':
01048         self.switch_to_cartesian_mode()
01049       elif not all([x == 'running' for x in self.cartesian_controllers_state]):
01050         rospy.logwarn("Cartesian controllers aren't running!  Attempting to start")
01051         self.start_cartesian_controllers()
01052       elif self.joint_controller_state == 'running':
01053         rospy.logwarn("joint controllers are running!  Attempting to stop")
01054         self.stop_controllers(stop_joint = 1)
01055 
01056 
01057   ##send a single joint configuration to the joint trajectory action (takes in a 7-list of angles, nonblocking)
01058   def command_joint(self, jointangles):
01059     self.check_controllers_ok('joint')
01060 
01061     goal = JointTrajectoryGoal()
01062     goal.trajectory.header.stamp = rospy.get_rostime()
01063 
01064     goal.trajectory.joint_names = self.joint_names
01065 
01066     point = JointTrajectoryPoint(jointangles, [0]*7, [0]*7, rospy.Duration(0.01))
01067     goal.trajectory.points = [point,]
01068 
01069     self.joint_action_client.send_goal(goal)
01070 
01071 
01072   ##send an entire joint trajectory to the joint trajectory action 
01073   def command_joint_trajectory(self, trajectory, max_joint_vel = 0.2, current_angles = None, blocking = 0, times_and_vels = None):
01074     self.check_controllers_ok('joint')
01075 
01076     goal = JointTrajectoryGoal()
01077     goal.trajectory.joint_names = self.joint_names
01078 
01079     #normalize the trajectory so the continuous joints angles are close to the current angles
01080     trajectory = self.normalize_trajectory(trajectory)
01081 
01082     #start the trajectory 0.05 s from now
01083     goal.trajectory.header.stamp = rospy.get_rostime() + rospy.Duration(0.05)
01084 
01085     #fill in the times and vels if not specified
01086     if times_and_vels == None:
01087 
01088       #get the current joint angles
01089       if current_angles == None:
01090         current_angles = list(self.get_current_arm_angles())
01091 
01092       #set the first trajectory point to the current position
01093       trajectory = [current_angles] + trajectory
01094 
01095       #find appropriate times and velocities for the trajectory
01096       (times, vels) = self.ik_utilities.trajectory_times_and_vels(trajectory, [max_joint_vel]*7)
01097     
01098     else:
01099       (times, vels) = times_and_vels
01100 
01101 #     print "trajectory:"
01102 #     for point in trajectory:
01103 #       print self.pplist(point)
01104 #     print "times:", self.pplist(times)
01105 #     print "vels:"
01106 #     for vel in vels:
01107 #       print self.pplist(vel)
01108 
01109     #fill in the trajectory message
01110     for ind in range(len(trajectory)):
01111       point = JointTrajectoryPoint(trajectory[ind], vels[ind], [0]*7, rospy.Duration(times[ind]))
01112       goal.trajectory.points.append(point)
01113 
01114     #send the goal
01115     self.joint_action_client.send_goal(goal)
01116     
01117     #if blocking, wait until it gets there
01118     if blocking:
01119       self.wait_joint_trajectory_done(timeout = rospy.Duration(times[-1]+5))
01120 
01121 
01122   ##check the state of the joint trajectory action (returns 1 if done, 0 otherwise) 
01123   def check_joint_trajectory_done(self):
01124     state = self.joint_action_client.get_state()
01125     return state > 1
01126 
01127 
01128   ##wait for the joint trajectory action to finish (returns 1 if succeeded, 0 otherwise)
01129   #timeout is a ROS duration; default (0) is infinite timeout
01130   #returns 0 if timed out, 1 if succeeded
01131   def wait_joint_trajectory_done(self, timeout = rospy.Duration(0)):
01132     finished = self.joint_action_client.wait_for_result(timeout)
01133     if not finished:
01134       return "timed out"
01135     state = self.joint_action_client.get_state()
01136     if state == 3:
01137       return "succeeded"
01138     return "other"
01139 
01140 
01141   ##tell the joint trajectory action to stop the arm where it is now
01142   def freeze_arm(self):
01143     current_angles = self.get_current_arm_angles()
01144     self.command_joint(current_angles)
01145 
01146 
01147   ##send a command to the gripper action 
01148   def command_gripper(self, position, max_effort, blocking = 0):
01149     if not self.gripper_controller_state == 'running':
01150       self.check_controller_states()
01151       if not self.gripper_controller_state == 'running':
01152         rospy.logwarn("gripper controller not running!  Attempting to start")
01153         self.start_gripper_controller()
01154 
01155     goal = Pr2GripperCommandGoal()
01156     goal.command.position = position
01157     goal.command.max_effort = max_effort
01158     self.gripper_action_client.send_goal(goal)
01159 
01160     #if blocking, wait for the gripper to finish
01161     if blocking:
01162       self.gripper_action_client.wait_for_result(rospy.Duration(4.0))
01163       time.sleep(.5)
01164     
01165 
01166   ##figure out which controllers are loaded/started (fills in self.joint_controller_state,
01167   #self.cartesian_controllers_state, and self.gripper_controller_state with 
01168   #'not loaded', 'stopped', or 'running'
01169   def check_controller_states(self):
01170     resp = self.list_controllers_service()
01171 
01172     if self.gripper_controller in resp.controllers:
01173       self.gripper_controller_state = resp.state[resp.controllers.index(self.gripper_controller)]
01174     else:
01175       self.gripper_controller_state = 'not loaded'
01176 
01177     if self.joint_controller in resp.controllers:
01178       self.joint_controller_state = resp.state[resp.controllers.index(self.joint_controller)]
01179     else:
01180       self.joint_controller_state = 'not loaded'
01181 
01182     self.cartesian_controllers_state = []
01183     for controller in self.cartesian_controllers:
01184       if controller not in resp.controllers:
01185         self.cartesian_controllers_state.append('not loaded')
01186       else:       
01187         self.cartesian_controllers_state.append(resp.state[resp.controllers.index(controller)])
01188 
01189 
01190   ##load the Cartesian controllers with the current set of params on the param server
01191   def load_cartesian_controllers(self):
01192     self.check_controller_states()
01193 
01194     for (controller, state) in zip(self.cartesian_controllers, self.cartesian_controllers_state):
01195       if state == 'not loaded':
01196         success = self.load_controller_service(controller)
01197         if not success:
01198           rospy.logerr("error in loading Cartesian controller!")
01199           break  
01200     else:
01201       rospy.loginfo("all Cartesian controllers loaded")
01202 
01203     self.check_controller_states()
01204 
01205 
01206   ##unload the Cartesian controllers (to re-load with new params)
01207   def unload_cartesian_controllers(self):
01208 
01209     self.check_controller_states()
01210     if any([x=='running' for x in self.cartesian_controllers_state]):
01211       self.stop_controllers(stop_cartesian = 1)
01212 
01213     if all([x=='not loaded' for x in self.cartesian_controllers_state]):
01214       return
01215 
01216     self.stop_cartesian_commands()
01217 
01218     cart_controllers_reversed = self.cartesian_controllers[:]
01219     cart_controllers_reversed.reverse()
01220     state_reversed = self.cartesian_controllers_state[:]
01221     state_reversed.reverse()
01222     for (controller, state) in zip(cart_controllers_reversed, state_reversed):
01223       if state == 'stopped':
01224         success = self.unload_controller_service(controller)
01225         if not success:
01226           rospy.logerr("error in unloading Cartesian controller!")
01227           break
01228     else:
01229       rospy.loginfo("Cartesian controllers unloaded")
01230     self.check_controller_states()
01231 
01232 
01233   ##load the joint controller with the current set of params on the param server
01234   def load_joint_controllers(self):
01235     self.check_controller_states()
01236     if self.joint_controller_state == 'not loaded':
01237       success = self.load_controller_service(self.joint_controller)
01238       if not success:
01239         rospy.logerr("error in loading joint controller!")
01240     else:
01241       rospy.loginfo("joint controller already loaded")
01242     self.stop_cartesian_commands()
01243         
01244     self.check_controller_states()
01245 
01246 
01247   ##unload the joint controller 
01248   def unload_joint_controllers(self):
01249     self.check_controller_states()
01250     if self.joint_controller_state == 'running':
01251       self.stop_controllers(stop_joint = 1)
01252     elif self.joint_controller_state == 'not loaded':
01253       return
01254 
01255     success = self.unload_controller_service(self.joint_controller)
01256     if not success:
01257       rospy.logerr("error in unloading joint controller!")
01258 
01259     self.check_controller_states()
01260     
01261 
01262   ##stops the joint controllers, starts the Cartesian ones (both need to be loaded already)
01263   def switch_to_cartesian_mode(self):
01264     self.set_desired_cartesian_to_current()
01265     self.check_controller_states()
01266     if all([x=='stopped' for x in self.cartesian_controllers_state]) and \
01267           self.joint_controller_state == 'running':
01268       success = self.switch_controller_service(self.cartesian_controllers, [self.joint_controller,], 2)
01269       if success:
01270         rospy.loginfo("switched joint to Cartesian successfully")
01271       else:
01272         rospy.logerr("switching joint to Cartesian failed")
01273     else:
01274       self.start_cartesian_controllers()
01275       self.stop_controllers(stop_joint = 1)
01276     self.check_controller_states()
01277 
01278 
01279   ##stops the Cartesian controllers, starts the joint ones (both need to be loaded already)
01280   def switch_to_joint_mode(self):
01281     self.check_controller_states()
01282     if self.joint_controller_state == 'stopped' and \
01283           any([x=='running' for x in self.cartesian_controllers_state]):
01284       success = self.switch_controller_service([self.joint_controller,], self.cartesian_controllers, 2)
01285       if success:
01286         rospy.loginfo("switched Cartesian to joint successfully")
01287       else:
01288         rospy.logerr("switching Cartesian to joint failed")
01289     else:
01290       self.start_joint_controllers()
01291       self.stop_controllers(stop_cartesian = 1)
01292       self.stop_cartesian_commands()
01293     self.check_controller_states()
01294 
01295 
01296   ##just start the joint controllers (need to be loaded already)
01297   def start_joint_controllers(self):
01298     self.check_controller_states()
01299     if self.joint_controller_state == 'stopped':
01300       success = self.switch_controller_service([self.joint_controller,], [], 2)
01301       if success:
01302         rospy.loginfo("started joint controller successfully")
01303       else:
01304         rospy.logerr("starting joint controller failed")
01305     elif self.joint_controller_state == 'not loaded':
01306       rospy.logerr("joint controller not loaded!")
01307     self.stop_cartesian_commands()
01308 
01309     self.check_controller_states()
01310 
01311 
01312   ##start the Cartesian controllers (need to be loaded already)
01313   def start_cartesian_controllers(self):
01314     self.set_desired_cartesian_to_current() 
01315     self.check_controller_states()
01316     if any([x=='not loaded' for x in self.cartesian_controllers_state]):
01317       print "not all Cartesian controllers are loaded!"
01318 
01319     elif any([x=='stopped' for x in self.cartesian_controllers_state]):
01320       success = self.switch_controller_service(self.cartesian_controllers, [], 2)
01321       if success:
01322         rospy.loginfo("started Cartesian controllers successfully")
01323       else:
01324         rospy.logerr("starting Cartesian controllers failed")
01325     self.check_controller_states()    
01326 
01327 
01328   ##start the gripper controller (needs to be loaded already)
01329   def start_gripper_controller(self):
01330     self.check_controller_states()
01331     if self.gripper_controller_state == 'stopped':
01332       success = self.switch_controller_service([self.gripper_controller,], [], 2)
01333       if success:
01334         rospy.loginfo("started gripper controller successfully")
01335       else:
01336         rospy.logerr("starting gripper controller failed")
01337     elif self.gripper_controller_state == 'not loaded':
01338       rospy.logerr("gripper controller isn't loaded!")
01339     self.check_controller_states()
01340 
01341 
01342   ##stop all controllers
01343   def stop_all_controllers(self):
01344       self.stop_controllers(stop_joint = 1, stop_cartesian = 1, stop_gripper = 1)
01345 
01346 
01347   ##stop controllers that are currently running
01348   def stop_controllers(self, stop_joint = 0, stop_cartesian = 0, stop_gripper = 0):
01349     self.check_controller_states()
01350 
01351     #stop Cartesian controllers
01352     if stop_cartesian and any([x=='running' for x in self.cartesian_controllers_state]):
01353       success = self.switch_controller_service([], self.cartesian_controllers, 2)
01354       if success:
01355         rospy.loginfo("stopped Cartesian controllers successfully")
01356       else:
01357         rospy.logerr("stopping Cartesian controllers failed")
01358 
01359     #stop joint controllers
01360     if stop_joint and self.joint_controller_state == 'running':
01361       success = self.switch_controller_service([], [self.joint_controller,], 2)
01362       if success:
01363         rospy.loginfo("stopped joint controller successfully")    
01364       else:
01365         rospy.logerr("stopping joint controller failed")  
01366 
01367     #stop gripper controller
01368     if stop_gripper and self.gripper_controller_state == 'running':
01369       success = self.switch_controller_service([], [self.gripper_controller,], 2)
01370       if success:
01371         rospy.loginfo("stopped gripper controller successfully")
01372       else:
01373         rospy.logerr("stopping gripper controller failed")
01374 
01375     self.check_controller_states()
01376 
01377 
01378   ##re-load the Cartesian controllers with new parameters (change cart_params before running)
01379   def reload_cartesian_controllers(self, set_params = 1):
01380     self.check_controller_states()
01381 
01382     #set the new params on the parameter server
01383     if set_params:
01384       self.cart_params.set_params()
01385 
01386     #if we're running the Cartesian controllers, let the joint controllers hold while we switch
01387     restart = 0
01388     if any([x=='running' for x in self.cartesian_controllers_state]):
01389       restart = 1
01390       self.switch_to_joint_mode()
01391 
01392     #unload the cartesian controllers
01393     self.unload_cartesian_controllers()
01394 
01395     #load the cartesian controllers
01396     self.load_cartesian_controllers()
01397 
01398     #switch back from joint to cartesian
01399     if restart:
01400       self.switch_to_cartesian_mode()
01401 
01402     self.check_controller_states()
01403 
01404 
01405   ##re-load the joint controller with gains a fraction of the defaults
01406   def reload_joint_controllers(self, fraction = 1.):
01407     self.check_controller_states()
01408     
01409     #set the new params on the parameter server
01410     self.joint_params.set_gains_fraction(fraction)
01411     
01412     #if we're running the joint controllers, let the Cartesian controllers hold while we switch
01413     restart = 0
01414     if self.joint_controller_state == 'running':
01415       restart = 1
01416       self.switch_to_cartesian_mode()
01417 
01418     #unload the joint controller
01419     self.unload_joint_controllers()
01420   
01421     #re-load the joint controllers
01422     self.load_joint_controllers()
01423 
01424     #switch back from Cartesian to joint
01425     if restart:
01426       self.switch_to_joint_mode()
01427 
01428     self.check_controller_states()
01429     
01430 
01431   ##return the current Cartesian pose of the gripper
01432   def return_cartesian_pose(self, frame = 'base_link'):
01433     start_time = rospy.get_rostime()
01434     while not rospy.is_shutdown():
01435       try:
01436         t = self.tf_listener.getLatestCommonTime(frame, self.whicharm+'_wrist_roll_link')
01437         (trans, rot) = self.tf_listener.lookupTransform(frame, self.whicharm+'_wrist_roll_link', t)
01438         break
01439       except (tf.Exception, tf.ExtrapolationException):
01440         rospy.sleep(0.5)
01441         current_time = rospy.get_rostime()
01442         rospy.logerr("waiting for a tf transform between %s and %s"%(frame, \
01443                                                    self.whicharm+'_wrist_roll_link'))
01444         if current_time - start_time > rospy.Duration(30.):
01445           rospy.logerr("return_cartesian_pose waited 30 seconds for a tf transform!  Returning None")
01446           return (None, None)
01447     return (list(trans), list(rot))
01448 
01449 
01450   ##print the current Cartesian pose of the gripper
01451   def print_cartesian_pose(self):
01452     (trans, rot) = self.return_cartesian_pose()
01453     print "Cartesian pose trans:", self.pplist(trans), "rot:", self.pplist(rot)
01454     print "rotation matrix:\n", self.ppmat(tf.transformations.quaternion_matrix(rot))
01455 
01456 
01457   ##pretty-print list to string
01458   def pplist(self, list):
01459     return ' '.join(['%5.3f'%x for x in list])
01460 
01461 
01462   ##pretty-print numpy matrix to string
01463   def ppmat(self, mat):
01464     str = ''
01465     for i in range(mat.shape[0]):
01466       for j in range(mat.shape[1]):
01467         str += '%2.3f\t'%mat[i,j]
01468       str += '\n'
01469     return str
01470 
01471 
01472 
01473 #sample test cases
01474 if __name__ == '__main__':
01475 
01476   def keypause():
01477     print "press enter to continue"
01478     raw_input()
01479 
01480   rospy.init_node('controller_manager', anonymous=True)
01481 
01482 #points 0-2 and 6-7 have IK solutions; 3-5 don't (but the cartesian-plain controllers will try to get you there anyway)
01483   points = [
01484     [0.5, -0.5, 0.8, 0.5, 0.0, 0.0, 0.5],
01485     [0.6, -0.2, 0.4, 0.0, 0.0, 0.5, 0.5],
01486     [0.2, -0.8, 0.4, 0.0, 0.5, 0.0, 0.5],
01487     [0.5, -0.5, 1.2, 0.5, 0.0, 0.0, 0.5],
01488     [0.6, -0.2, 1.2, 0.0, 0.0, 0.5, 0.5],
01489     [0.2, -0.8, 1.2, 0.0, 0.5, 0.0, 0.5],
01490     [.62, -.05, .65, -0.5, 0.5, 0.5, 0.5],
01491     [.62, -.05, .56, -0.5, 0.5, 0.5, 0.5]
01492   ]
01493 
01494   zeros = [0]*7
01495   sideangles = [-0.447, -0.297, -2.229, -0.719, 0.734, -1.489, 1.355]
01496 
01497   cm = ControllerManager('r')
01498 
01499   test_angles = [
01500   [-0.094, 0.711, -0.000, -1.413, -2.038, -1.172, -0.798],
01501   [0.126, 0.832, 0.000, -1.740, -2.977, -1.091, 3.043]]
01502 
01503   print "going through test_angles and waiting for the trajectory to finish"
01504   cm.command_joint_trajectory(test_angles)
01505   result = cm.wait_joint_trajectory_done(rospy.Duration(30.0))
01506   print result
01507 
01508   # print "starting gripper controller"
01509   # cm.start_gripper_controller()
01510 
01511   print "switching to cartesian controllers"
01512   cm.switch_to_cartesian_mode()
01513 
01514   print "moving to point 6 and waiting for it to get there"
01515   cm.command_cartesian(points[6])
01516   result = cm.wait_cartesian_really_done(points[6], .01, .1, rospy.Duration(30.0), rospy.Duration(15.0))
01517   print result
01518   print "desired pose:", cm.pplist(points[6])
01519   cm.print_cartesian_pose()
01520   keypause()
01521 
01522   print "going to all 0 angles with joint controllers, testing contingency-switch"
01523   cm.command_joint(zeros)
01524   keypause()
01525 
01526   # print "opening gripper"
01527   # cm.command_gripper(0.08, -1.0)
01528   # keypause()
01529 
01530   # cm.start_joint_controllers()
01531 
01532   # print "going to near-side-grasp angles"
01533   # cm.command_joint(sideangles)
01534   # keypause()
01535 
01536   # print "commanding an interpolated IK trajectory to go from side approach to side grasp"
01537   # start_angles = sideangles
01538   # tiltangle = math.pi/18.
01539   # sideapproachmat = numpy.array([[0., -1., 0., 0.],  
01540   #                                [math.cos(tiltangle), 0., math.sin(tiltangle), 0.],
01541   #                                [-math.sin(tiltangle), 0., math.cos(tiltangle), 0.],
01542   #                                [0., 0., 0., 1.]])
01543   # sideapproachpos = [.62, -.3, .6]
01544   # approachmat = sideapproachmat
01545   # approachpos = sideapproachpos
01546   # approachquat = list(tf.transformations.quaternion_from_matrix(approachmat))
01547   # sidegrasppos = sideapproachpos[:]
01548   # sidegrasppos[1] += .05
01549   # grasppos = sidegrasppos
01550   # graspquat = approachquat[:]
01551 
01552   # start_pose = create_pose_stamped(approachpos+approachquat)
01553   # end_pose = create_pose_stamped(grasppos+graspquat)
01554   # cm.command_interpolated_ik(end_pose, start_pose)   
01555   # keypause()
01556 
01557   # print "closing gripper (blocking)"
01558   # cm.command_gripper(0.0, 50.0, 1)
01559   # print "done"
01560 
01561   print "moving to point 0 in Cartesian mode, testing contingency switch"
01562   cm.command_cartesian(points[0])
01563   keypause()
01564   cm.print_cartesian_pose()
01565 
01566   if cm.use_trajectory_cartesian:
01567     print "reloading cartesian controllers with zero PID terms (should be unable to move)"
01568     cm.cart_params.pose_fb_trans_p = 0.
01569     cm.cart_params.pose_fb_trans_d = 0.
01570     cm.cart_params.pose_fb_rot_p = 0.
01571     cm.cart_params.pose_fb_rot_d = 0.
01572     cm.reload_cartesian_controllers()
01573   else:
01574     print "setting cartesian gains to near-zero (should be unable to move)"
01575     cm.cart_params.set_gains([0.001]*3, [0.001]*3, [0.001]*3, [0.001]*3)
01576 
01577   print "trying to move to point 2 (should fail)"
01578   cm.command_cartesian(points[2])
01579   keypause()
01580 
01581   if cm.use_trajectory_cartesian:
01582     print "reloading cartesian controllers with normal PID terms"
01583     cm.cart_params.pose_fb_trans_p=800.
01584     cm.cart_params.pose_fb_trans_d=12.
01585     cm.cart_params.pose_fb_rot_p=80.
01586     cm.cart_params.pose_fb_rot_d=0.0
01587     cm.reload_cartesian_controllers()
01588   else:
01589     print "setting cartesian gains back"
01590     cm.cart_params.set_params_to_defaults()
01591     cm.cart_params.set_gains()
01592 
01593   print "trying again to move to point 2 (should succeed)"
01594   cm.command_cartesian(points[2])
01595   keypause()
01596 
01597   # print "starting joint controllers and going to all 0 angles"
01598   # cm.command_joint([0,0,0,0,0,0,0])
01599   # keypause()
01600 
01601   # print "stopping all controllers"
01602   # cm.stop_all_controllers()
01603 
01604 
01605 


pr2_gripper_reactive_approach
Author(s): Kaijen Hsiao
autogenerated on Mon Oct 6 2014 12:27:12