test_careobot_st_jla_ca_torus.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 #
00003 # Copyright 2017 Fraunhofer Institute for Manufacturing Engineering and Automation (IPA)
00004 #
00005 # Licensed under the Apache License, Version 2.0 (the "License");
00006 # you may not use this file except in compliance with the License.
00007 # You may obtain a copy of the License at
00008 #
00009 #   http://www.apache.org/licenses/LICENSE-2.0
00010 #
00011 # Unless required by applicable law or agreed to in writing, software
00012 # distributed under the License is distributed on an "AS IS" BASIS,
00013 # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014 # See the License for the specific language governing permissions and
00015 # limitations under the License.
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 # has to be startet with ns param: rosrun cob_twist_controller collect_twist_control_eval_data.py __ns:=arm_right
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 
00042 #     cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_LEAST_SINGULAR_VALUE)
00043 #     cli.set_config_param(tcc.LAMBDA_MAX, 0.2)
00044 #     cli.set_config_param(tcc.EPS_DAMP, 0.2)
00045     cli.set_config_param(tcc.EPS_TRUNC, 0.001)
00046 
00047     cli.set_config_param(tcc.PRIO_CA, 100)
00048     cli.set_config_param(tcc.PRIO_JLA, 50)
00049 
00050     cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS)
00051     cli.set_config_param(tcc.K_H, 1.0)
00052 
00053     cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
00054     cli.set_config_param(tcc.K_H_CA, -2.0)
00055     cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
00056     cli.set_config_param(tcc.ACTIV_BUF_CA, 50.0)
00057     cli.set_config_param(tcc.CRIT_THRESH_CA, 0.025)
00058     cli.set_config_param(tcc.DAMP_CA, 0.000001)
00059 
00060     cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
00061     cli.set_config_param(tcc.K_H_JLA, -1.0)
00062     cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
00063     cli.set_config_param(tcc.ACTIV_BUF_JLA, 300.0)
00064     cli.set_config_param(tcc.CRIT_THRESH_JLA, 5.0)
00065     cli.set_config_param(tcc.DAMP_JLA, 0.00001)
00066 
00067     cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
00068     cli.set_config_param(tcc.KEEP_DIR, True)
00069     cli.set_config_param(tcc.ENF_VEL_LIM, True)
00070     cli.set_config_param(tcc.ENF_POS_LIM, True)
00071 
00072     cli.update()
00073     cli.close()
00074 
00075     cli = Client('frame_tracker')
00076     ft_param = {'cart_min_dist_threshold_lin' : 0.2, 'cart_min_dist_threshold_rot' : 0.2}
00077     cli.update_configuration(ft_param)
00078     cli.close()
00079 
00080 
00081 def init_pos():
00082     sss = simple_script_server()
00083     sss.move("arm_right", "home")
00084     sss.move("arm_right", [[-0.039928546971938594, 0.7617065276699337, 0.01562043859727158, -1.7979678396373568, 0.013899422367821046, 1.0327319085987252, 0.021045294231647915]])
00085 
00086 
00087 if __name__ == "__main__":
00088     rospy.init_node("test_careobot_st_jla_ca_torus")
00089 
00090     base_dir = '/home/fxm-mb/bag-files/2015_08_06/'
00091     if rospy.has_param('~base_dir'):
00092         base_dir = rospy.get_param('~base_dir')
00093     else:
00094         rospy.logwarn('Could not find parameter ~base_dir. Using default base_dir: ' + base_dir)
00095 
00096     if rospy.has_param('chain_tip_link'):
00097         chain_tip_link = rospy.get_param('chain_tip_link')
00098     else:
00099         rospy.logwarn('Could not find parameter chain_tip_link.')
00100         exit(-1)
00101 
00102     if rospy.has_param('frame_tracker/tracking_frame'):
00103         tracking_frame = rospy.get_param('frame_tracker/tracking_frame')
00104     else:
00105         rospy.logwarn('Could not find parameter frame_tracker/tracking_frame.')
00106         exit(-2)
00107 
00108     if rospy.has_param('root_frame'):
00109         root_frame = rospy.get_param('root_frame')
00110     else:
00111         rospy.logwarn('Could not find parameter root_frame.')
00112         exit(-3)
00113 
00114     t = time.localtime()
00115     launch_time_stamp = time.strftime("%Y%m%d_%H_%M_%S", t)
00116     command = 'rosbag play ' + base_dir + 'careobot_st_jla_ca.bag'
00117 
00118     data_krakens = [
00119                     JointStateDataKraken(base_dir + 'careobot_st_jla_ca_joint_state_data_' + launch_time_stamp + '.csv'),
00120                     ObstacleDistanceDataKraken(base_dir + 'careobot_st_jla_ca_obst_dist_data_' + launch_time_stamp + '.csv'),
00121                     TwistDataKraken(base_dir + 'careobot_st_jla_ca_twist_data_' + launch_time_stamp + '.csv'),
00122                     JointVelocityDataKraken(base_dir + 'careobot_st_jla_ca_joint_vel_data_' + launch_time_stamp + '.csv'),
00123                     FrameTrackingDataKraken(base_dir + 'careobot_st_jla_ca_frame_tracking_data_' + launch_time_stamp + '.csv', root_frame, chain_tip_link, tracking_frame), ]
00124 
00125     init_pos()
00126     init_dyn_recfg()
00127 
00128     status_open = True
00129     for data_kraken in data_krakens:
00130         status_open = status_open and data_kraken.open()
00131     if status_open:
00132         rospy.loginfo('Subscribers started for data collection ... \nPress CTRL+C to stop program and write data to the file.')
00133 
00134         traj_marker_command = 'rosrun cob_twist_controller debug_trajectory_marker_node __ns:=' + rospy.get_namespace()
00135         traj_marker_pid = subprocess.Popen(traj_marker_command, stdin = subprocess.PIPE, shell = True)
00136         pid = subprocess.Popen(command, stdin = subprocess.PIPE, cwd = base_dir, shell = True)
00137 
00138         # pid.wait()
00139         time.sleep(1.5)  # give time to switch mode back
00140 
00141         if traj_marker_pid.poll() is not None:
00142             rospy.logerr("traj_marker_pid returned code. Aborting ...")
00143             pid.send_signal(subprocess.signal.SIGINT)
00144             pid.kill()
00145             # traj_marker_pid.send_signal(subprocess.signal.SIGINT)
00146             traj_marker_pid.send_signal(subprocess.signal.CTRL_C_EVENT)
00147             traj_marker_pid.kill()
00148             exit()
00149 
00150         time.sleep(0.5)  # give time to switch mode back
00151 
00152         rate = rospy.Rate(10)
00153         try:
00154             while not rospy.is_shutdown():
00155                 rate.sleep()
00156                 if(pid.poll() is not None):
00157                     rospy.loginfo('Bag file finished stop script.')
00158                     break
00159 
00160         except (KeyboardInterrupt, SystemExit) as e:
00161             rospy.loginfo('KeyboardInterrupt / SystemExit: ' + str(e))
00162             # save data
00163             for data_kraken in data_krakens:
00164                 data_kraken.writeAllData()
00165         except rospy.ROSInterruptException as e:
00166             rospy.logwarn('ROSInterruptException: ' + str(e))
00167         except:
00168             rospy.logerr('Else exception.')
00169         else:
00170             for data_kraken in data_krakens:
00171                 data_kraken.writeAllData()
00172 
00173         try:
00174             # pid.send_signal(subprocess.signal.SIGINT)
00175             pid.kill()
00176             pid.send_signal(subprocess.signal.SIGINT)
00177         except Exception as e:
00178             rospy.logerr('Failed to stop rosbag play due to exception: ' + str(e))
00179         try:
00180             traj_marker_pid.kill()
00181             traj_marker_pid.send_signal(subprocess.signal.SIGINT)
00182         except Exception as e:
00183             rospy.logerr('Failed to stop debug_trajectory_marker_node due to exception: ' + str(e))
00184     else:
00185         rospy.logerr('Failed to open DataKraken files.')
00186 
00187 
00188 
00189 
00190 
00191 
00192 


cob_twist_controller
Author(s): Felix Messmer , Marco Bezzon , Christoph Mark , Francisco Moreno
autogenerated on Thu Jun 6 2019 21:19:26