kelsey_sandbox: test_vel_move.py Source File

Go to the documentation of this file.
00001 #! /usr/bin/python
00002 
00003 from khpr2import import *
00004 
00005 def main():
00006     rospy.init_node("test_vel_move")
00007 
00008     possible_controllers = ['%s_joint_controller_low']
00009     ctrl_switcher = ControllerSwitcher()
00010     if True:
00011         ctrl_switcher.carefree_switch('r', '%s_arm_controller', 
00012                            '$(find hrl_pr2_arms)/params/joint_traj_params_electric.yaml')
00013         rospy.sleep(0.5)
00014         ctrl_switcher.carefree_switch('r', '%s_joint_controller_low', 
00015                            '$(find hrl_pr2_arms)/params/joint_traj_params_electric_low.yaml')
00016     r_arm_js = create_pr2_arm('r', PR2ArmJointTrajectory, controller_name='%s_joint_controller_low')
00017     q = [-0.34781704,  0.27341079, -1.75392154, -2.08626393, -3.43756314, -1.82146607, -1.85187734]
00018     r_arm_js.set_ep(q, 3) 
00019     rospy.sleep(6)
00020     ctrl_switcher.carefree_switch('r', '%s_cart',
00021                                   '$(find kelsey_sandbox)/params/j_transpose_low_rfh.yaml')
00022 
00023     r_arm = create_pr2_arm('r', PR2ArmCartesianPostureBase)
00024     r_arm.set_posture()
00025     rospy.sleep(0.2)
00026     pos_i_des, rot_i_des = r_arm.get_ep()
00027     pos_i_act, rot_i_act = r_arm.get_end_effector_pose()
00028     pos_err = pos_i_act - pos_i_des
00029     rot_err = rot_i_act * rot_i_des.T
00030     time_start = rospy.get_time()
00031 #pos_cur = pos_start.copy()
00032 #rot_cur = rot_start.copy()
00033 #pos_end = pos_start - np.mat([0.15, 0, 0]).T
00034 #pos_lead = np.mat([0, 0, 0]).T
00035     test_pub = rospy.Publisher("/test", Float64MultiArray)
00036     xd_des = 0.015
00037     xd_integ = 1.0
00038     vels = deque([0]*40)
00039     while not rospy.is_shutdown():
00040         pos, rot = r_arm.get_end_effector_pose()
00041         xd_act = r_arm.get_controller_state()['xd_act'][0]
00042         vels.append(xd_act)
00043         vels.popleft()
00044         xd_act_filt = np.mean(vels)
00045         xd_integ += (xd_des - xd_act_filt)
00046 #k_p = 0.14
00047 #k_i = 0.01
00048 #k_c = 0.02
00049         k_p = 0.014
00050         k_i = 0.0015
00051         k_c = 0.003
00052         xd_ctrl = k_p * (xd_des - xd_act_filt) + k_i * xd_integ + np.sign(xd_des) * k_c
00053         xd_ctrl_final = np.clip(xd_ctrl, -0.30, 0.30)
00054         msg = Float64MultiArray()
00055         msg.data = [xd_ctrl, k_p * (xd_des - xd_act_filt), k_i * xd_integ, np.sign(xd_des) * k_c]
00056         test_pub.publish(msg)
00057         print "ctrl", xd_ctrl, "err", (xd_des - xd_act), "integ", xd_integ, "final", xd_ctrl_final
00058         pos_des = pos + np.mat([xd_ctrl_final, 0, 0]).T - pos_err
00059         rot_des = rot * rot_err.T
00060         pos_des[1:,0] = pos_i_des[1:,0]
00061         rot_des = rot_i_des
00062 
00063         r_arm.set_ep((pos_des, rot_des), 1)
00064         rospy.sleep(0.01)
00065     r_arm.set_ep(r_arm.get_ep(), 1)
00066     pos_f_act, rot_f_act = r_arm.get_end_effector_pose()
00067     time_end = rospy.get_time()
00068     print (pos_f_act[0] - pos_i_act[0]) / (time_end - time_start)
00069 
00070 if __name__ == "__main__":
00071     main()