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("cool_force_demo")
00029
00030 from optparse import OptionParser
00031 p = OptionParser()
00032 p.add_option('-f', '--force_mag', dest="force_mag", default=2,
00033 help="Specify force magnitude.")
00034 p.add_option('-x', '--max_force', dest="max_force", default=-1,
00035 help="Specify max force magnitude.")
00036 p.add_option('-c', '--compliance', dest="compliance", default=-1,
00037 help="Compliance to maintain.")
00038 p.add_option('-t', '--tip_frame', dest="tip_frame", default="/l_gripper_tool_frame",
00039 help="Set tip to this frame.")
00040 p.add_option('-z', '--zero_sensor', dest="zero_sensor", default=False,
00041 action="store_true", help="Just zero the sensor.")
00042 p.add_option('-l', '--force_line', dest="force_line", default=False,
00043 action="store_true", help="Move in a line with zero force.")
00044 p.add_option('-p', '--force_plane', dest="force_plane", default=False,
00045 action="store_true", help="Move in a plane with zero force.")
00046 p.add_option('-o', '--force_point', dest="force_point", default=False,
00047 action="store_true", help="Move about a point with zero torque.")
00048 p.add_option('-r', '--force_roll', dest="force_roll", default=False,
00049 action="store_true", help="Move the wrist with zero torque.")
00050 p.add_option('-a', '--force_all', dest="force_all", default=False,
00051 action="store_true", help="All DOF are trying to set zero force.")
00052 p.add_option('-n', '--force_none', dest="force_none", default=False,
00053 action="store_true", help="Return to position control.")
00054 p.add_option('-k', '--kill_controller', dest="kill_controller", default=False,
00055 action="store_true", help="Render controller dead.")
00056 (opts, args) = p.parse_args()
00057
00058 arm = create_pr2_arm('l', PR2ArmHybridForce)
00059 rospy.sleep(1)
00060
00061 if opts.zero_sensor:
00062 arm.zero_sensor()
00063
00064
00065 arm.set_ep(arm.get_end_effector_pose(), 0)
00066 arm.set_force(6 * [0])
00067
00068 fp_trans_major = 3
00069 fp_trans_minor = 1
00070 fp_rot = 0.1
00071 fi_trans_major = 0.002
00072 fi_trans_minor = 0.001
00073 fi_max_trans_major = 10
00074 fi_max_trans_minor = 5
00075 fi_rot = 0
00076 fi_max_rot = 0
00077 pp_trans = 1000
00078 pd_trans_major = 16
00079 pd_trans_minor = 4
00080 pp_rot = 120
00081 pd_rot = 0
00082
00083 if opts.force_line:
00084 arm.set_force_max([float(opts.max_force), -1, -1, -1, -1, -1])
00085 arm.set_tip_frame(opts.tip_frame)
00086 arm.set_force_gains(p_trans=[fp_trans_major, fp_trans_minor, fp_trans_minor],
00087 p_rot=fp_rot, i_trans=[fi_trans_major, fi_trans_minor, fi_trans_minor],
00088 i_max_trans=[fi_max_trans_major, fi_max_trans_minor, fi_max_trans_minor],
00089 i_rot=fi_rot, i_max_rot=fi_max_rot)
00090 arm.set_motion_gains(p_trans=[float(opts.compliance), pp_trans, pp_trans],
00091 d_trans=[pd_trans_major, pd_trans_minor, pd_trans_minor],
00092 p_rot=pp_rot, d_rot=pd_rot)
00093 arm.set_force_directions(['x'])
00094 arm.set_force([float(opts.force_mag), 0, 0, 0, 0, 0])
00095 arm.update_gains()
00096 return
00097
00098 if opts.force_plane:
00099 arm.set_force_max([-1, float(opts.max_force), float(opts.max_force), -1, -1, -1, -1])
00100 arm.set_tip_frame(opts.tip_frame)
00101 arm.set_force_gains(p_trans=[fp_trans_minor, fp_trans_major, fp_trans_major],
00102 p_rot=fp_rot, i_trans=[fi_trans_minor, fi_trans_major, fi_trans_major],
00103 i_max_trans=[fi_max_trans_minor, fi_max_trans_major, fi_max_trans_major],
00104 i_rot=fi_rot, i_max_rot=fi_max_rot)
00105 arm.set_motion_gains(p_trans=[pp_trans, float(opts.compliance), float(opts.compliance)],
00106 d_trans=[pd_trans_minor, pd_trans_major, pd_trans_major],
00107 p_rot=pp_rot, d_rot=pd_rot)
00108 arm.set_force_directions(['y', 'z'])
00109 arm.set_force([0, float(opts.force_mag), float(opts.force_mag), 0, 0, 0])
00110 arm.update_gains()
00111 return
00112
00113 if opts.force_point:
00114 arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
00115 arm.set_tip_frame(opts.tip_frame)
00116 arm.set_force_gains(p_trans=[fp_trans_minor, fp_trans_minor, fp_trans_minor],
00117 p_rot=0.8, i_trans=[fi_trans_minor, fi_trans_minor, fi_trans_minor],
00118 i_max_trans=[fi_max_trans_minor, fi_max_trans_minor, fi_max_trans_minor],
00119 i_rot=fi_rot, i_max_rot=fi_max_rot)
00120 arm.set_motion_gains(p_trans=[pp_trans, pp_trans, pp_trans],
00121 d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor],
00122 p_rot=pp_rot, d_rot=pd_rot)
00123 arm.set_force_directions([0, 0, 0, 1, 1, 1])
00124 arm.set_force([0, 0, 0, float(opts.force_mag), float(opts.force_mag), float(opts.force_mag)])
00125 arm.update_gains()
00126 return
00127
00128 if opts.force_roll:
00129 arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
00130 arm.set_tip_frame(opts.tip_frame)
00131 arm.set_force_gains(p_trans=[fp_trans_minor, fp_trans_minor, fp_trans_minor],
00132 p_rot=1.8, i_trans=[fi_trans_minor, fi_trans_minor, fi_trans_minor],
00133 i_max_trans=[fi_max_trans_minor, fi_max_trans_minor, fi_max_trans_minor],
00134 i_rot=fi_rot, i_max_rot=fi_max_rot)
00135 arm.set_motion_gains(p_trans=[pp_trans, pp_trans, pp_trans],
00136 d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor],
00137 p_rot=pp_rot, d_rot=pd_rot)
00138 arm.set_force_directions([0, 0, 0, 1, 0, 0])
00139 arm.set_force([0, 0, 0, float(opts.force_mag), 0, 0])
00140 arm.update_gains()
00141 return
00142
00143 if opts.force_all:
00144 arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
00145 arm.set_tip_frame(opts.tip_frame)
00146 arm.set_force_gains(p_trans=6,
00147 p_rot=1.8, i_trans=[fi_trans_major, fi_trans_major, fi_trans_major],
00148 i_max_trans=[fi_max_trans_major, fi_max_trans_major, fi_max_trans_major],
00149 i_rot=fi_rot, i_max_rot=fi_max_rot)
00150 arm.set_motion_gains(p_trans=[pp_trans, pp_trans, pp_trans],
00151 d_trans=[pd_trans_major, pd_trans_major, pd_trans_major],
00152 p_rot=pp_rot, d_rot=pd_rot)
00153 arm.set_force_directions([1, 1, 1, 1, 1, 1])
00154 arm.set_force([0, 0, 0, 0, 0, 0])
00155 arm.update_gains()
00156 return
00157
00158 if opts.force_none:
00159 arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
00160 arm.set_tip_frame(opts.tip_frame)
00161 arm.set_force_gains(p_trans=[0, 0, 0],
00162 p_rot=0, i_trans=[0, 0, 0],
00163 i_max_trans=[0, 0, 0],
00164 i_rot=0, i_max_rot=0)
00165 arm.set_motion_gains(p_trans=[pp_trans, pp_trans, pp_trans],
00166 d_trans=[pd_trans_minor, pd_trans_minor, pd_trans_minor],
00167 p_rot=pp_rot, d_rot=pd_rot)
00168 arm.set_force_directions([0, 0, 0, 0, 0, 0])
00169 arm.set_force([0, 0, 0, 0, 0, 0])
00170 arm.update_gains()
00171 return
00172
00173 if opts.kill_controller:
00174 arm.set_force_max([-1, -1, -1, -1, -1, -1, -1])
00175 arm.set_tip_frame(opts.tip_frame)
00176 arm.set_force_gains(p_trans=[0, 0, 0],
00177 p_rot=0, i_trans=[0, 0, 0],
00178 i_max_trans=[0, 0, 0],
00179 i_rot=0, i_max_rot=0)
00180 arm.set_motion_gains(p_trans=0,
00181 d_trans=0,
00182 p_rot=0, d_rot=0)
00183 arm.set_force_directions([0, 0, 0, 0, 0, 0])
00184 arm.set_force([0, 0, 0, 0, 0, 0])
00185 arm.update_gains()
00186 return
00187
00188 if __name__ == "__main__":
00189 main()