00001
00002
00003 import sys
00004 import numpy as np
00005
00006 import roslib
00007 roslib.load_manifest('hrl_pr2_arms')
00008 roslib.load_manifest('smach_ros')
00009 roslib.load_manifest('actionlib')
00010 import rospy
00011 import rosbag
00012
00013 import smach
00014 from smach_ros import SimpleActionState, ServiceState, IntrospectionServer
00015 import actionlib
00016 import tf
00017 import tf.transformations as tf_trans
00018 from std_msgs.msg import Bool, Float32
00019 from std_srvs.srv import Empty
00020 from geometry_msgs.msg import PoseStamped, Vector3, Pose
00021 from actionlib_msgs.msg import GoalStatus
00022
00023 from hrl_generic_arms.pose_converter import PoseConverter
00024 from hrl_pr2_arms.pr2_arm import create_pr2_arm
00025 from hrl_pr2_arms.pr2_arm_hybrid import PR2ArmHybridForce
00026
00027 def main():
00028 rospy.init_node("switch_controller")
00029
00030 from optparse import OptionParser
00031 p = OptionParser()
00032 p.add_option('-s', '--stiff', dest="stiff", default=False,
00033 action="store_true", help="Enable stiff controller.")
00034 p.add_option('-f', '--force', dest="force", default=False,
00035 action="store_true", help="Enable force controller.")
00036 p.add_option('-m', '--force_mag', dest="force_mag", default=2,
00037 help="Specify force magnitude.")
00038 p.add_option('-x', '--max_force', dest="max_force", default=-1,
00039 help="Specify max force magnitude.")
00040 p.add_option('-i', '--impedance', dest="impedance", default=False,
00041 action="store_true", help="Enable impedance controller.")
00042 p.add_option('-c', '--compliance', dest="compliance", default=-1,
00043 help="Enable impedance controller.")
00044 p.add_option('-t', '--tip_frame', dest="tip_frame", default="/l_gripper_tool_frame",
00045 help="Set tip to this frame.")
00046 p.add_option('-z', '--zero_sensor', dest="zero_sensor", default=False,
00047 action="store_true", help="Just zero the sensor.")
00048 p.add_option('-r', '--reset_pose', dest="reset_pose", default=False,
00049 action="store_true", help="Use the saved position in the data file.")
00050 (opts, args) = p.parse_args()
00051
00052 arm = create_pr2_arm('l', PR2ArmHybridForce)
00053 rospy.sleep(0.1)
00054
00055
00056 arm.zero_sensor()
00057 if opts.zero_sensor:
00058 return
00059 arm.set_force(6 * [0])
00060
00061
00062 if opts.reset_pose:
00063 pkg_dir = roslib.rospack.rospackexec(["find", "hrl_phri_2011"])
00064 bag = rosbag.Bag(pkg_dir + "/data/saved_teleop_pose.bag", 'r')
00065 for topic, msg, stamp in bag.read_messages("/teleop_pose"):
00066 pose = PoseConverter.to_pos_rot([msg.position.x, msg.position.y, msg.position.z],
00067 [msg.orientation.x, msg.orientation.y, msg.orientation.z,
00068 msg.orientation.w])
00069 for topic, msg, stamp in bag.read_messages("/teleop_posture"):
00070 posture = msg.data
00071 bag.close()
00072 arm.set_posture(posture)
00073 i_poses = arm.interpolate_ep(arm.get_end_effector_pose(), pose, 100)
00074 for cur_pose in i_poses:
00075 arm.set_ep(cur_pose, 1)
00076 rospy.sleep(0.1)
00077 return
00078
00079
00080 arm.set_force_max([float(opts.max_force), -1, -1, -1, -1, -1])
00081 arm.set_tip_frame(opts.tip_frame)
00082
00083 if opts.stiff:
00084 compliance = float(opts.compliance)
00085 if compliance < 0:
00086 compliance = 1300
00087
00088 arm.set_force_gains(p_trans=[1, 0, 0], p_rot=0.1, i_trans=[0.002, 0, 0], i_max_trans=[10, 0, 0], i_rot=0, i_max_rot=0)
00089 arm.set_motion_gains(p_trans=[compliance, 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0)
00090 arm.set_force_directions([])
00091 arm.set_force(6 * [0])
00092 elif opts.impedance:
00093 compliance = float(opts.compliance)
00094 if compliance < 0:
00095 compliance = 80
00096 arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0)
00097 arm.set_motion_gains(p_trans=[compliance, 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0)
00098 arm.set_force_directions([])
00099 arm.set_force(6 * [0])
00100 elif opts.force:
00101 arm.set_force_gains(p_trans=[3, 1, 1], p_rot=0.1, i_trans=[0.002, 0.001, 0.001], i_max_trans=[10, 5, 5], i_rot=0, i_max_rot=0)
00102 arm.set_motion_gains(p_trans=[float(opts.compliance), 1300, 1300], d_trans=[16, 10, 10], p_rot=120, d_rot=0)
00103 arm.set_force_directions(['x'])
00104 arm.set_force([float(opts.force_mag), 0, 0, 0, 0, 0])
00105 else:
00106 p.print_help()
00107 return
00108 arm.update_gains()
00109
00110 if __name__ == "__main__":
00111 main()