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