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()
