test_careobot_st_jla_ca_selfcollision_arm_left_direct_head.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     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)  # change priorities now CA constraint has highest prio
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     # sss.move("arm_right", [[-0.039928546971938594, 0.7617065276699337, 0.01562043859727158, -1.7979678396373568, 0.013899422367821046, 1.0327319085987252, 0.021045294231647915]])
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)  # give time to switch mode back
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             # save data
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             # pid.send_signal(subprocess.signal.SIGINT)
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 


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