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.1)
00053 cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0)
00054 cli.set_config_param(tcc.CRIT_THRESH_CA, 0.03)
00055
00056 cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
00057 cli.set_config_param(tcc.K_H_JLA, -1.0)
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", [[-0.17566952192977148, 0.6230441162870415, 0.27476319388433623, -1.1163222472275676, 0.3166667900974902, 0.5333301416131739, -0.32441227738211875], ])
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_internal_motions.bag'
00113
00114 data_krakens = [
00115 JointStateDataKraken(base_dir + 'internal_motion_joint_state_data_' + launch_time_stamp + '.csv'),
00116 ObstacleDistanceDataKraken(base_dir + 'internal_motion_obst_dist_data_' + launch_time_stamp + '.csv'),
00117 TwistDataKraken(base_dir + 'internal_motion_twist_data_' + launch_time_stamp + '.csv'),
00118 JointVelocityDataKraken(base_dir + 'internal_motion_joint_vel_data_' + launch_time_stamp + '.csv'),
00119 FrameTrackingDataKraken(base_dir + 'internal_motion_frame_tracking_data_' + launch_time_stamp + '.csv', root_frame, chain_tip_link, tracking_frame), ]
00120
00121 init_pos()
00122 init_dyn_recfg()
00123
00124
00125
00126
00127
00128
00129 status_open = True
00130 for data_kraken in data_krakens:
00131 status_open = status_open and data_kraken.open()
00132 if status_open:
00133 rospy.loginfo('Subscribers started for data collection ... \nPress CTRL+C to stop program and write data to the file.')
00134 pid = subprocess.Popen(command, stdin = subprocess.PIPE, cwd = base_dir, shell = True)
00135 time.sleep(0.5)
00136 rate = rospy.Rate(10)
00137 try:
00138 while not rospy.is_shutdown():
00139 rate.sleep()
00140 if(pid.poll() is not None):
00141 rospy.loginfo('Bag file finished stop script.')
00142 break
00143
00144 except (KeyboardInterrupt, SystemExit) as e:
00145 rospy.loginfo('KeyboardInterrupt / SystemExit: ' + str(e))
00146
00147 for data_kraken in data_krakens:
00148 data_kraken.writeAllData()
00149 except rospy.ROSInterruptException as e:
00150 rospy.logwarn('ROSInterruptException: ' + str(e))
00151 except:
00152 rospy.logerr('Else exception.')
00153 else:
00154 for data_kraken in data_krakens:
00155 data_kraken.writeAllData()
00156 try:
00157
00158 pid.kill()
00159 pid.send_signal(subprocess.signal.SIGINT)
00160 except Exception as e:
00161 rospy.logerr('Failed to stop rosbag play due to exception: ' + str(e))
00162 else:
00163 rospy.logerr('Failed to open DataKraken files.')
00164
00165