00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018 import time
00019 import rospy
00020 import subprocess
00021
00022 from simple_script_server.simple_script_server import simple_script_server
00023 import twist_controller_config as tcc
00024 from dynamic_reconfigure.client import Client
00025
00026 from data_collection import JointStateDataKraken
00027 from data_collection import ObstacleDistanceDataKraken
00028 from data_collection import TwistDataKraken
00029 from data_collection import JointVelocityDataKraken
00030 from data_collection import FrameTrackingDataKraken
00031
00032
00033 def init_dyn_recfg():
00034 cli = tcc.TwistControllerReconfigureClient()
00035 cli.init()
00036
00037 cli.set_config_param(tcc.NUM_FILT, False)
00038 cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY)
00039 cli.set_config_param(tcc.LAMBDA_MAX, 0.1)
00040 cli.set_config_param(tcc.W_THRESH, 0.05)
00041 cli.set_config_param(tcc.EPS_TRUNC, 0.001)
00042
00043 cli.set_config_param(tcc.PRIO_CA, 50)
00044 cli.set_config_param(tcc.PRIO_JLA, 100)
00045
00046 cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS)
00047 cli.set_config_param(tcc.K_H, 1.0)
00048
00049 cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
00050 cli.set_config_param(tcc.K_H_CA, -1.0)
00051 cli.set_config_param(tcc.DAMP_CA, 0.000001)
00052 cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.2)
00053 cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0)
00054 cli.set_config_param(tcc.CRIT_THRESH_CA, 0.1)
00055
00056 cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
00057 cli.set_config_param(tcc.K_H_JLA, -0.5)
00058 cli.set_config_param(tcc.DAMP_JLA, 0.000001)
00059 cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
00060 cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0)
00061 cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0)
00062
00063 cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
00064 cli.set_config_param(tcc.KEEP_DIR, True)
00065 cli.set_config_param(tcc.ENF_VEL_LIM, True)
00066 cli.set_config_param(tcc.ENF_POS_LIM, True)
00067
00068 cli.update()
00069 cli.close()
00070
00071 cli = Client('frame_tracker')
00072 ft_param = {'cart_min_dist_threshold_lin' : 0.2, 'cart_min_dist_threshold_rot' : 0.2}
00073 cli.update_configuration(ft_param)
00074 cli.close()
00075
00076
00077 def init_pos():
00078 sss = simple_script_server()
00079
00080 sss.move("arm_left", [[2.274847163975995, -0.08568661652370757, -0.8273207226405264, -1.9404110013940103, 1.306315205401929, 1.5627416614617742, -1.1199725164070955], ])
00081
00082
00083 if __name__ == "__main__":
00084 rospy.init_node("test_careobot_st_jla_ca_selfcollision_arm_left")
00085
00086 base_dir = '/home/fxm-mb/bag-files/arm_left_selfcollision/'
00087 if rospy.has_param('~base_dir'):
00088 base_dir = rospy.get_param('~base_dir')
00089 else:
00090 rospy.logwarn('Could not find parameter ~base_dir. Using default base_dir: ' + base_dir)
00091
00092 if rospy.has_param('chain_tip_link'):
00093 chain_tip_link = rospy.get_param('chain_tip_link')
00094 else:
00095 rospy.logwarn('Could not find parameter chain_tip_link.')
00096 exit(-1)
00097
00098 if rospy.has_param('frame_tracker/tracking_frame'):
00099 tracking_frame = rospy.get_param('frame_tracker/tracking_frame')
00100 else:
00101 rospy.logwarn('Could not find parameter frame_tracker/tracking_frame.')
00102 exit(-2)
00103
00104 if rospy.has_param('root_frame'):
00105 root_frame = rospy.get_param('root_frame')
00106 else:
00107 rospy.logwarn('Could not find parameter root_frame.')
00108 exit(-3)
00109
00110 t = time.localtime()
00111 launch_time_stamp = time.strftime("%Y%m%d_%H_%M_%S", t)
00112 command = 'rosbag play ' + base_dir + 'arm_left_to_head_direct_JLA.bag'
00113
00114 data_krakens = [
00115 JointStateDataKraken(base_dir + 'careobot_st_jla_sca_joint_state_data_' + launch_time_stamp + '.csv'),
00116 ObstacleDistanceDataKraken(base_dir + 'careobot_st_jla_sca_obst_dist_data_' + launch_time_stamp + '.csv'),
00117 TwistDataKraken(base_dir + 'careobot_st_jla_sca_twist_data_' + launch_time_stamp + '.csv'),
00118 JointVelocityDataKraken(base_dir + 'careobot_st_jla_sca_joint_vel_data_' + launch_time_stamp + '.csv'),
00119 FrameTrackingDataKraken(base_dir + 'careobot_st_jla_sca_frame_tracking_data_' + launch_time_stamp + '.csv', root_frame, chain_tip_link, tracking_frame), ]
00120
00121 init_pos()
00122 init_dyn_recfg()
00123
00124 status_open = True
00125 for data_kraken in data_krakens:
00126 status_open = status_open and data_kraken.open()
00127 if status_open:
00128 rospy.loginfo('Subscribers started for data collection ... \nPress CTRL+C to stop program and write data to the file.')
00129 pid = subprocess.Popen(command, stdin = subprocess.PIPE, cwd = base_dir, shell = True)
00130 time.sleep(0.5)
00131 rate = rospy.Rate(10)
00132 try:
00133 while not rospy.is_shutdown():
00134 rate.sleep()
00135 if(pid.poll() is not None):
00136 rospy.loginfo('Bag file finished stop script.')
00137 break
00138
00139 except (KeyboardInterrupt, SystemExit) as e:
00140 rospy.loginfo('KeyboardInterrupt / SystemExit: ' + str(e))
00141
00142 for data_kraken in data_krakens:
00143 data_kraken.writeAllData()
00144 except rospy.ROSInterruptException as e:
00145 rospy.logwarn('ROSInterruptException: ' + str(e))
00146 except:
00147 rospy.logerr('Else exception.')
00148 else:
00149 for data_kraken in data_krakens:
00150 data_kraken.writeAllData()
00151 try:
00152
00153 pid.kill()
00154 pid.send_signal(subprocess.signal.SIGINT)
00155 except Exception as e:
00156 rospy.logerr('Failed to stop rosbag play due to exception: ' + str(e))
00157 else:
00158 rospy.logerr('Failed to open DataKraken files.')
00159
00160