class CalibrationSetupHelper: def __init__(self, robot_description, base_link, arm_root_link, arm_tip_link, head_root_link, head_tip_link, arm_controller, head_controller, head_camera_frame, head_camera_joint, camera_namespace): self.robot_description = robot_description self.base_link = base_link self.arm_root_link = arm_root_link self.arm_tip_link = arm_tip_link self.head_root_link = head_root_link self.head_tip_link = head_tip_link self.arm_controller = arm_controller self.head_controller = head_controller self.head_camera_frame = head_camera_frame self.head_camera_joint = head_camera_joint self.camera_namespace = camera_namespace self.robot = URDF().parse(self.robot_description) self.robot_name = self.robot.name.lower() if not self.base_link: self.base_link = self.robot.get_root() if self.robot.link_map.has_key(self.base_link) == False: self.error_tip_link(self.base_link, '--base-link') if self.robot.link_map.has_key(self.arm_root_link) == False: self.error_tip_link(self.arm_root_link, '--arm-root-link') if self.robot.link_map.has_key(self.head_root_link) == False: self.error_tip_link(self.head_root_link, '--head-root-link') if self.robot.link_map.has_key(self.arm_tip_link) == False: self.error_tip_link(self.arm_tip_link, '--arm-tip-link') if self.robot.link_map.has_key(self.head_tip_link) == False: self.error_tip_link(self.head_tip_link, '--head-tip-link') if self.robot.link_map.has_key(self.head_camera_frame) == False: self.error_tip_link(self.head_camera_frame, '--head-camera-frame') if self.robot.joint_map.has_key(self.head_camera_joint) == False: self.error_joint(self.head_camera_joint, '--head-camera-joint') all_chain = [] for limb, base_link, end_link in [('arm', self.arm_root_link, self.arm_tip_link), ('head', self.head_root_link, self.head_tip_link)]: all_chain.append(self.robot.get_chain(base_link, end_link)[1:]) for c1,c2 in itertools.product(*all_chain): if c1 == c2 : rospy.logerr('arm/head chain share same joint ({}), this will cause failure'.format(c1)) sys.exit() for limb, base_link, end_link in [('arm', self.arm_root_link, self.arm_tip_link), ('head', self.head_root_link, self.head_tip_link)]: joint_list = filter(lambda x: self.robot.joint_map.has_key(x) and self.robot.joint_map[x].type != 'fixed', [c for c in self.robot.get_chain(base_link, end_link)]) exec('self.{0}_joint_list = {1}'.format(limb, joint_list)) in locals() rospy.loginfo('using following joint for {} chain'.format('arm')) rospy.loginfo('... {}'.format(self.arm_joint_list)) rospy.loginfo('using following joint for {} chain'.format('head')) rospy.loginfo('... {}'.format(self.head_joint_list)) # create robot_calibration directory self.dirname_top = self.robot_name+'_calibration' self.dirname_capture = self.robot_name+'_calibration/capture_data' self.dirname_estimate = self.robot_name+'_calibration/estimate_params' self.dirname_results = self.robot_name+'_calibration/view_results' try: os.makedirs(self.dirname_top) os.makedirs(self.dirname_capture) os.makedirs(self.dirname_estimate) os.makedirs(self.dirname_results) except Exception as e: rospy.logfatal(e) #sys.exit(-1) # setup capture_data self.write_all_pipelines(args.use_kinect) # all_pipelines.launch self.write_all_viewers(args.use_kinect) # all_viewers.launch self.write_capture_data() # capture_data.launch self.write_capture_exec() # capture_exec.launch self.write_hardware_config() # hardware_config self.write_make_samples() # make_samples.py # setup estimate_params self.write_estimation_config() # estimation_config.launch self.write_calibrate_sh() # calibrate_<robot>.sh self.write_estimate_params_config(args.use_kinect) # free_arms.yaml free_cameras.yaml free_cb_locations.yaml free_torso.yaml system.yaml # setup view_results self.write_view_results() # scatter_config.yaml view_scatter.sh def error_tip_link(self, link, option): rospy.logfatal('could not find valid link name ... {}'.format(link)) rospy.logfatal('use {0} from one of following names {1}'.format(option, map(lambda x: x.name, self.robot.links))) f = open('/tmp/{0}.xml'.format(self.robot_name), 'w+') print >>f, self.robot_description f.close() rospy.logfatal('try \'urdf_to_graphiz /tmp/{}.xml\'.'.format(self.robot_name)) sys.exit(-1) def error_joint(self, joint, option): rospy.logfatal('could not find valid joint name ... {}'.format(joint)) rospy.logfatal('use {0} from one of following names {1}'.format(option, map(lambda x: x.name, self.robot.joints))) f = open('/tmp/{0}.xml'.format(self.robot_name), 'w+') print >>f, self.robot_description f.close() rospy.logfatal('try \'urdf_to_graphiz /tmp/{}.xml\'.'.format(self.robot_name)) sys.exit(-1) def write_all_pipelines(self, use_kinect): file_name = self.dirname_capture+'/all_pipelines.launch' f = open(file_name,'w+') f.write(""" <launch> <include file="$(find calibration_launch)/capture_data/chain_settler.launch" ns="arm_chain" /> <include file="$(find calibration_launch)/capture_data/chain_settler.launch" ns="head_chain" /> <arg name="use_kinect" default="{0}" /> <group if="$(arg use_kinect)" > <include file="$(find calibration_launch)/capture_data/kinect_pipeline.launch" ns="{1}"> <arg name="image_topic" value="image_rect_color"/> <!-- openni2.launch publishes image_raw and image_rect_color --> <arg name="depth_topic" value="/camera/depth/image"/> <!-- this is floar value --> <arg name="camera_info_topic" value="camera_info"/> </include> </group> <group unless="$(arg use_kinect)" > <include file="$(find calibration_launch)/capture_data/monocam_pipeline.launch" ns="{1}"> <arg name="image_topic" value="image_rect_color"/> <!-- openni2.launch publishes image_raw and image_rect_color --> </include> </group> <node type="interval_intersection_action" pkg="interval_intersection" name="interval_intersection" output="screen"> <remap from="head_chain" to="head_chain/settled_interval" /> <remap from="arm_chain" to="arm_chain/settled_interval" /> <remap from="head_camera" to="{1}/settled_interval" /> </node> </launch> """.format("true" if use_kinect else "false", self.camera_namespace)) f.close() def write_all_viewers(self, use_kinect): file_name = self.dirname_capture+'/all_viewers.launch' f = open(file_name,'w+') f.write(""" <launch> <!-- Hack to create the directory --> <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/{0}_calibration" /> <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/{0}_calibration/cb_fail" /> <param name="{2}/annotated_viewer/filename_format" type="string" value="/tmp/{0}_calibration/cb_fail/cb_{1}_%04i.jpg" /> <include file="$(find calibration_launch)/capture_data/annotated_viewer.launch" ns="{2}" > <arg name="image_topic" value="image_rect_annotated" /> </include> </launch> """.format(self.robot_name,"kinect" if use_kinect else "monocam", self.camera_namespace)) f.close() def write_capture_data(self): file_name = self.dirname_capture+'/capture_data.launch' f = open(file_name,'w+') f.write(""" <launch> <include file="$(find {0}_calibration)/capture_data/all_viewers.launch"/> <include file="$(find {0}_calibration)/capture_data/all_pipelines.launch"/> <include file="$(find {0}_calibration)/capture_data/capture_exec.launch"/> </launch> """.format(self.robot_name)) def write_capture_exec(self): file_name = self.dirname_capture+'/capture_exec.launch' f = open(file_name,'w+') f.write(""" <launch> <node type="capture_exec.py" pkg="calibration_launch" name="calibration_exec" args="$(find {0}_calibration)/capture_data/samples/ $(find {0}_calibration)/capture_data/hardware_config $(find {0}_calibration)/estimate_params/config/system.yaml" output="screen" > <remap from="head_camera/camera_info" to="{1}/camera_info"/> <remap from="head_camera/image_rect" to="{1}/image_rect_throttle"/> <remap from="head_camera/image" to="{1}/image_rect_throttle"/> <remap from="head_camera/features" to="{1}/features"/> </node> <node type="urdf_pub.py" pkg="calibration_launch" name="urdf_pub"/> <node type="record" pkg="rosbag" name="robot_measurement_recorder" output="screen" args="-O /tmp/{0}_calibration/cal_measurements robot_measurement robot_description" > <!-- Hack to create the directory --> <param name="mkdir_tmp" command="mkdir -m 777 -p /tmp/{0}_calibration" /> </node> </launch>""".format(self.robot_name, self.camera_namespace)) f.close() def write_hardware_config(self): config_dir = self.dirname_capture+'/hardware_config/' try: os.makedirs(config_dir) except Exception as e: rospy.logfatal(e) # cam_config.yaml f = open(config_dir+'/cam_config.yaml', 'w+') f.write(""" # ----- Microsoft Kinect ----- head_camera: ###FIX cb_detector_config: {0}/cb_detector_config ## FIXME led_detector_config: {0}/led_detector settler_config: {0}/monocam_settler_config ## FIXME configs: small_cb_4x5: settler: tolerance: 2.00 ignore_failures: True max_step: 3.0 cache_size: 100 cb_detector: active: True num_x: 4 num_y: 5 width_scaling: 0.5 height_scaling: 0.5 subpixel_window: 4 subpixel_zero_zone: 1 led_detector: active: False """.format(self.camera_namespace)) # not sure if this is works for mono camera f.close() # chain_config.yaml f = open(config_dir+'/chain_config.yaml', 'w+') for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): if len(joint_list) == 0: continue tolerance = 0.002 f.write('# ----- {} -----\n'.format(limb)) f.write('{}_chain:\n'.format(limb)) f.write(' settler_config: /{}_chain/settler_config\n\n'.format(limb)) f.write(' configs:\n') f.write(' tight_tol:\n') f.write(' settler:\n') f.write(' joint_names:\n') for j in joint_list: f.write(' - {}\n'.format(j)) f.write(' tolerances:\n') for j in joint_list: f.write(' - {}\n'.format(tolerance)) f.write(' max_step: 1.0\n') f.write(' cache_size: 1500\n\n') f.close() # controller_config.yaml f = open(config_dir+'/controller_config.yaml', 'w+') for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): if len(joint_list) == 0: continue f.write('{}_controller:\n'.format(limb)) exec('controller = self.{}_controller'.format(limb)) rospy.loginfo('Subscribing controller topic is \'{}\''.format(controller)) f.write(' topic: {0}\n'.format(controller)) f.write(' joint_names:\n') for j in joint_list: f.write(' - {}\n'.format(j)) f.close() # laser_config.yaml f = open(config_dir+'/laser_config.yaml', 'w+') f.write('# laser config') f.close() def write_make_samples(self): samples_dir = self.dirname_capture+'/samples/' try: os.makedirs(samples_dir+'/arm') except Exception as e: rospy.logfatal(e) f = open(samples_dir+'/arm/config.yaml', 'w+') f.write(""" group: "Arm" prompt: "Please put the checkerboard in the hand (open/close the gripper with the joystick's square/circle buttons)." finish: "Skipping arm samples" repeat: False """) f.close() f = open(samples_dir+'/../make_samples.py', 'w+') os.chmod(samples_dir+'/../make_samples.py', 0744) f.write("""#!/usr/bin/env python # capture samples!!!!1!one # this script should eventually be replaced by something that finds # samples automatically import rospy from sensor_msgs.msg import JointState import string, os header1 = \"\"\"camera_measurements: - {cam_id: head_camera, config: small_cb_4x5} joint_commands: \"\"\" """) for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): if len(joint_list) == 0: continue f.write(""" header2_{0} = \"\"\"- controller: {0}_controller segments: - duration: 2.0 positions: \"\"\" """.format(limb)) f.write(""" header3 = \"\"\"joint_measurements: """) for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): if len(joint_list) == 0: continue f.write('- {{chain_id: {0}_chain, config: tight_tol}}\n'.format(limb)) f.write(""" sample_id: arm_\"\"\" header4 = \"\"\"target: {chain_id: arm_chain, target_id: small_cb_4x5}\"\"\" class SampleMaker: def __init__(self): rospy.init_node("make_samples") rospy.Subscriber("joint_states", JointState, self.callback) """) for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): if len(joint_list) == 0: continue f.write(' self.{}_joints = ['.format(limb)) for j in joint_list: f.write('\"{}\", '.format(j)) f.write(']\n') f.write(' self.{0}_state = [0.0 for joint in self.{0}_joints]\n'.format(limb)) f.write(""" self.count = 0 while not rospy.is_shutdown(): print "Move arm/head to a new sample position." resp = raw_input("press <enter> ") if string.upper(resp) == "EXIT": break else: # save a sample: count = str(self.count).zfill(4) f = open(os.path.dirname(__file__)+"/samples/arm/arm_"+count+".yaml", "w") f.write(header1) """) f.write(""" print('saving ... {0}'.format(self.count))\n""") for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): if len(joint_list) == 0: continue f.write(""" print(' {0}_state: {{0}}'.format(self.{0}_state))\n""".format(limb)) f.write(""" f.write(header2_{0}) print>>f, self.{0}_state """.format(limb)) f.write(""" f.write(header3) print>>f, count f.write(header4) self.count += 1 def callback(self, msg): """) for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): if len(joint_list) == 0: continue f.write(""" for i in range(len(self.{0}_joints)): try: idx = msg.name.index(self.{0}_joints[i]) self.{0}_state[i] = msg.position[idx] except: pass """.format(limb)) f.write(""" if __name__=="__main__": SampleMaker() """) f.close() def write_estimation_config(self): file_name = self.dirname_estimate+'/estimation_config.launch' f = open(file_name,'w+') f.write(""" <launch> <group ns="calibration_config" clear_params="true"> <rosparam file="$(find {0}_calibration)/estimate_params/config/system.yaml" command="load" /> <group ns="cal_steps"> <group ns="{0} - 00 - Estimating Checkerboard Locations"> <param name="free_params" textfile="$(find {0}_calibration)/estimate_params/config/free_cb_locations.yaml" /> <param name="use_cov" type="bool" value="False" /> <rosparam> sensors: - arm_chain - head_camera </rosparam> <param name="output_filename" type="string" value="config_0" /> </group> <group ns="{0} - 01 - Adding Camera Locations"> <param name="free_params" textfile="$(find {0}_calibration)/estimate_params/config/free_cameras.yaml" /> <param name="use_cov" type="bool" value="True" /> <rosparam> sensors: - arm_chain - head_camera </rosparam> <param name="output_filename" type="string" value="system_calibrated" /> </group> </group> </group> </launch> """.format(self.robot_name)) def write_calibrate_sh(self): file_name = self.dirname_estimate+'/calibrate_'+self.robot_name+'.sh' f = open(file_name,'w+') os.chmod(file_name, 0744) f.write("""#! /bin/bash if [ -f robot_calibrated.xml ]; then echo "./robot_calibrated.xml already exists. Either back up this file or remove it before continuing" exit 1 fi echo "Checking if we can write to ./robot_calibrated.xml..." touch robot_calibrated.xml if [ "$?" -ne "0" ]; then echo "Not able to write to ./robot_calibrated.xml" echo "Make sure you run this script from a directory that for which you have write permissions." exit 1 fi rm robot_calibrated.xml echo "Success" roslaunch {0}_calibration estimation_config.launch rosrun calibration_estimation multi_step_cov_estimator.py /tmp/{0}_calibration/cal_measurements.bag /tmp/{0}_calibration __name:=cal_cov_estimator est_return_val=$? if [ "$est_return_val" -ne "0" ]; then echo "Estimator exited prematurely with error code [$est_return_val]" exit 1 fi # Make all the temporary files writable chmod ag+w /tmp/{0}_calibration/* """.format(self.robot_name)) def write_estimate_params_config(self, use_kinect=False): config_dir = self.dirname_estimate+'/config' try: os.makedirs(config_dir) except Exception as e: rospy.logfatal(e) # free_arms.yaml f = open(config_dir+'/free_arms.yaml', 'w+') f.write('\n') f.write(' transforms:\n') for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): if len(joint_list) == 0: continue for j in joint_list: f.write(' {0}: [ 0, 0, 0, {1}, {2}, {3} ]\n'.format(self.robot.joint_map[j].name, self.robot.joint_map[j].axis[0], self.robot.joint_map[j].axis[1], self.robot.joint_map[j].axis[2])) f.write('\n') f.write(' chains:\n') for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): if len(joint_list) == 0: continue f.write(' {}_chain:\n'.format(limb)) f.write(' gearing: {}\n'.format([0]*len(joint_list))) f.write(""" rectified_cams: head_camera: baseline_shift: 0 f_shift: 0 cx_shift: 0 cy_shift: 0 tilting_lasers: {} checkerboards: small_cb_4x5: spacing_x: 0 spacing_y: 0 """) f.close() # free_cameras.yaml f = open(config_dir+'/free_cameras.yaml', 'w+') f.write(""" transforms: arm_chain_cb: [1, 1, 1, 1, 1, 1] {0}: [1, 1, 1, 1, 1, 1] chains: """.format(self.head_camera_joint)) for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): if len(joint_list) == 0: continue f.write(' {}_chain:\n'.format(limb)) f.write(' gearing: {}\n'.format([0]*len(joint_list))) f.write(""" rectified_cams: head_camera: baseline_shift: 0 f_shift: 0 cx_shift: 0 cy_shift: 0 tilting_lasers: {} checkerboards: small_cb_4x5: spacing_x: 0 spacing_y: 0 """) f.close() # free_cb_locations.yaml f = open(config_dir+'/free_cb_locations.yaml', 'w+') f.write(""" transforms: arm_chain_cb: [1, 1, 1, 1, 1, 1] chains: """) for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): if len(joint_list) == 0: continue f.write(' {}_chain:\n'.format(limb)) f.write(' gearing: {}\n'.format([0]*len(joint_list))) f.write(""" rectified_cams: head_camera: baseline_shift: 0 f_shift: 0 cx_shift: 0 cy_shift: 0 tilting_lasers: {} checkerboards: small_cb_4x5: spacing_x: 0 spacing_y: 0 """) # free_torso.yaml f = open(config_dir+'/free_torso.yaml', 'w+') f.write(""" transforms: head_joint: [0, 0, 1, 0, 0, 0] chains: """) for limb, joint_list in zip(['arm', 'head'], [self.arm_joint_list, self.head_joint_list]): if len(joint_list) == 0: continue f.write(' {}_chain:\n'.format(limb)) f.write(' gearing: {}\n'.format([0]*len(joint_list))) f.write(""" rectified_cams: head_camera: baseline_shift: 0 f_shift: 0 cx_shift: 0 cy_shift: 0 tilting_lasers: {} checkerboards: small_cb_4x5: spacing_x: 0 spacing_y: 0 """) # system.yaml f = open(config_dir+'/system.yaml', 'w+') f.write('\n') f.write('base_link: {0}\n'.format(self.base_link)) f.write('\n') f.write('sensors:\n') f.write('\n') f.write(' chains:\n') if len(self.arm_joint_list) > 0: f.write(' arm_chain:\n') f.write(' root: {}\n'.format(self.arm_root_link)) f.write(' tip: {}\n'.format(self.arm_tip_link)) f.write(' cov:\n') f.write(' joint_angles: {}\n'.format([0.01]*len(self.arm_joint_list))) f.write(' gearing: {}\n'.format([1.0]*len(self.arm_joint_list))) if len(self.head_joint_list) > 0: f.write(' head_chain:\n') f.write(' root: {}\n'.format(self.head_root_link)) f.write(' tip: {}\n'.format(self.head_tip_link)) f.write(' cov:\n') f.write(' joint_angles: {}\n'.format([0.01]*len(self.head_joint_list))) f.write(' gearing: {}\n'.format([1.0]*len(self.head_joint_list))) f.write('\n') f.write(' rectified_cams:\n') f.write(' head_camera:\n') f.write(' chain_id: head_chain #TODO: get rid of this\n') f.write(' frame_id: {}\n'.format(self.head_camera_frame)) f.write(' baseline_shift: 0.0\n') if use_kinect: f.write(' baseline_rgbd: 0.075\n') else: f.write('# baseline_rgbd: 0.075 ## comment out if we run all_pipelines.launch with use_kinect\n') f.write(' f_shift: 0.0\n') f.write(' cx_shift: 0.0\n') f.write(' cy_shift: 0.0\n') f.write(' cov: {u: 0.25, v: 0.25, x: 0.25}\n') f.write('\n') f.write(' tilting_lasers: {}\n') f.write('\n') f.write('checkerboards:\n') f.write(' small_cb_4x5:\n') f.write(' corners_x: 4\n') f.write(' corners_y: 5\n') f.write(' spacing_x: 0.0245\n') f.write(' spacing_y: 0.0245\n') f.write('\n') f.write('transforms:\n') f.write(' arm_chain_cb: [ .25, 0, 0, pi/2, 0, 0]\n') f.write('\n') def write_view_results(self): file_name = self.dirname_results+'/scatter_config.yaml' f = open(file_name, 'w+') f.write('- name: "Head Camera: Arm"\n') f.write(' 3d: arm_chain\n') f.write(' cam: head_camera\n') f.write(' plot_ops:\n') f.write(' color: g\n') f.write(' marker: s\n') f.close() file_name = self.dirname_results+'/view_scatter.sh' f = open(file_name,'w+') os.chmod(file_name, 0744) f.write("""#!/bin/bash rosrun calibration_estimation error_visualization.py /tmp/{0}_calibration/cal_measurements.bag /tmp/{0}_calibration/ `rospack find {0}_calibration`/view_results/scatter_config.yaml\n""".format(self.robot_name)) f.close()
class Hybrid(): def __init__(self): rospy.init_node('hybrid_node') self.freq = 100 self.rate = rospy.Rate(self.freq) # 100 hz # pub self.pub_test = rospy.Publisher('/hybrid/test', String) # sub self.sub_jr3 = rospy.Subscriber('/jr3/wrench', WrenchStamped, self.cb_jr3) self.sub_joy = rospy.Subscriber('/spacenav/joy', Joy, self.cb_joy) self.sub_enable = rospy.Subscriber('/hybrid/enable', Bool, self.cb_enable) self.sub_cmd = rospy.Subscriber('/hybrid/cmd', String, self.cb_cmd) # tf self.ler = tf.TransformListener() # listener self.ber = tf.TransformBroadcaster() # broadcaster # datas self.enabled = False self.cmdfrm = Frame() self.wrench = Wrench() self.cmdtwist = Twist() self.urdf = rospy.get_param('/wam/robot_description') print self.urdf self.robot = URDF() self.robot = self.robot.from_xml_string(self.urdf) self.chain = self.robot.get_chain('base_link', 'cutter_tip_link') print self.chain pass def cb_jr3(self, msg): self.wrench.force = Vector(msg.wrench.force.x, msg.wrench.force.y, msg.wrench.force.z) self.wrench.torque = Vector(msg.wrench.torque.x, msg.wrench.torque.y, msg.wrench.torque.z) pass def cb_joy(self, msg): # might want to rotate this twist # self.cmdtwist.vel = Vector(msg.axes[0], msg.axes[1], msg.axes[2]) # self.cmdtwist.rot = Vector(msg.axes[3], msg.axes[4], msg.axes[5]) # align twist to be same as base link self.cmdtwist.vel = Vector(msg.axes[2], msg.axes[1], -msg.axes[0]) self.cmdtwist.rot = Vector(msg.axes[5], msg.axes[4], -msg.axes[3]) # print self.cmdtwist pass def cb_enable(self, msg): self.enabled = msg.data if msg.data: # sync cmd pos/rot rospy.loginfo("enable hybrid force control") try: (trans, rot) = \ self.ler.lookupTransform('wam/base_link', 'wam/cutter_tip_link', rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print "tf read error" self.cmdfrm.p = Vector(trans[0], trans[1], trans[2]) self.cmdfrm.M = Rotation.Quaternion(rot[0], rot[1], rot[2], rot[3]) print self.cmdfrm pass def cb_cmd(self, msg): pass def ctrl_hybrid(self): # massage cmd frm with force controller # z: force control on z axis cmdforce = -4 # -4N on z axis errforce = cmdforce - self.wrench.force.z() print self.wrench.force pass def run(self): while not rospy.is_shutdown(): msg = String() msg.data = 'yeah' self.pub_test.publish(msg) # update states # option 1: direct # option 2: hybrid if self.enabled: gain_vel = 0.1 gain_rot = 0.5 self.cmdtwist.vel *= gain_vel self.cmdtwist.rot *= gain_rot # self.cmdfrm.Integrate(self.cmdtwist, self.freq) tmptwist = self.cmdfrm.M.Inverse(self.cmdtwist) self.cmdfrm.Integrate(tmptwist, self.freq) # hybrid self.ctrl_hybrid() cmdtrans = (self.cmdfrm.p.x(), self.cmdfrm.p.y(), self.cmdfrm.p.z()) cmdrot = self.cmdfrm.M.GetQuaternion() self.ber.sendTransform(cmdtrans, cmdrot, rospy.Time.now(), "wam/cmd", "wam/base_link") # print cmdTrans self.rate.sleep()