def __init__(self, name, bot):
# pass in the name of the head component
self.bot = bot
c = m3t.get_component_config_filename(name)
self.config_name = c[:-4] + "_toolbox.yml"
try:
f = file(self.config_name, "r")
self.config = yaml.safe_load(f.read())
except (IOError, EOFError):
print "Config file not present:", self.config_name
return
self.joints = range(7) # lower 7Dof
self.joint_names = {
"NeckTilt": 0,
"NeckPan": 1,
"HeadRoll": 2,
"HeadTilt": 3,
"EyeTilt": 4,
"EyePanRight": 5,
"EyePanLeft": 6,
}
fcr = m3t.get_m3_ros_config_path()[-1] + "cameras/" + self.config["right_camera_calibration"] + ".yml"
fcl = m3t.get_m3_ros_config_path()[-1] + "cameras/" + self.config["left_camera_calibration"] + ".yml"
try:
f = file(fcr, "r")
cr = yaml.safe_load(f.read())
except (IOError, EOFError):
print "Config file not present:", fcr
return
try:
f = file(fcl, "r")
cl = yaml.safe_load(f.read())
except (IOError, EOFError):
print "Config file not present:", fcl
return
pr = cr["projection_matrix"]["data"]
pl = cl["projection_matrix"]["data"]
# extract calibration data for camera. should really do this with ROS image_geometry package
self.camera_calib = {
"right": {
"fx": pr[0],
"fy": pr[5],
"cx": pr[2],
"cy": pr[6],
"w": cr["image_width"],
"h": cr["image_height"],
},
"left": {
"fx": pl[0],
"fy": pl[5],
"cx": pl[2],
"cy": pl[6],
"w": cl["image_width"],
"h": cl["image_height"],
},
}
def __init__(self, name, bot):
M3HeadToolboxS2.__init__(self, name, bot)
fcm = m3t.get_m3_ros_config_path(
)[-1] + 'cameras/' + self.config['middle_camera_calibration'] + '.yml'
try:
f = file(fcm, 'r')
cm = yaml.safe_load(f.read())
except (IOError, EOFError):
print 'Config file not present:', fcm
return
pm = cm['projection_matrix']['data']
#extract calibration data for camera. should really do this with ROS image_geometry package
self.camera_calib['middle'] = {
'fx': pm[0],
'fy': pm[5],
'cx': pm[2],
'cy': pm[6],
'w': cm['image_width'],
'h': cm['image_height']
}
P = self.bot.list_to_kdl_vector(
self.config['middle_camera__translation_in_toolframe'])
R = self.bot.list_to_kdl_rotation(
self.config['middle_camera_rotation_in_toolframe'])
self.tool_T_xe = PyKDL.Frame(
R, P) #Fixed transform from middle eye frame to toolframe of head
self.xe_T_tool = self.tool_T_xe.Inverse()
def __init__(self, name, bot):
#pass in the name of the head component
self.bot = bot
c = m3t.get_component_config_filename(name)
self.config_name = c[:-4] + '_toolbox.yml'
try:
f = file(self.config_name, 'r')
self.config = yaml.safe_load(f.read())
except (IOError, EOFError):
print 'Config file not present:', self.config_name
return
self.joints = range(7) #lower 7Dof
self.joint_names = {
'NeckTilt': 0,
'NeckPan': 1,
'HeadRoll': 2,
'HeadTilt': 3,
'EyeTilt': 4,
'EyePanRight': 5,
'EyePanLeft': 6
}
fcr = m3t.get_m3_ros_config_path(
)[-1] + 'cameras/' + self.config['right_camera_calibration'] + '.yml'
fcl = m3t.get_m3_ros_config_path(
)[-1] + 'cameras/' + self.config['left_camera_calibration'] + '.yml'
try:
f = file(fcr, 'r')
cr = yaml.safe_load(f.read())
except (IOError, EOFError):
print 'Config file not present:', fcr
return
try:
f = file(fcl, 'r')
cl = yaml.safe_load(f.read())
except (IOError, EOFError):
print 'Config file not present:', fcl
return
pr = cr['projection_matrix']['data']
pl = cl['projection_matrix']['data']
#extract calibration data for camera. should really do this with ROS image_geometry package
self.camera_calib={'right':{'fx':pr[0],'fy':pr[5],'cx':pr[2],'cy':pr[6],'w':cr['image_width'],'h':cr['image_height']},\
'left':{'fx':pl[0],'fy':pl[5],'cx':pl[2],'cy':pl[6] ,'w':cl['image_width'],'h':cl['image_height']}}
def __init__(self, name, bot):
M3HeadToolboxS2.__init__(self, name, bot)
fcm = m3t.get_m3_ros_config_path()[-1] + "cameras/" + self.config["middle_camera_calibration"] + ".yml"
try:
f = file(fcm, "r")
cm = yaml.safe_load(f.read())
except (IOError, EOFError):
print "Config file not present:", fcm
return
pm = cm["projection_matrix"]["data"]
# extract calibration data for camera. should really do this with ROS image_geometry package
self.camera_calib["middle"] = {
"fx": pm[0],
"fy": pm[5],
"cx": pm[2],
"cy": pm[6],
"w": cm["image_width"],
"h": cm["image_height"],
}
P = self.bot.list_to_kdl_vector(self.config["middle_camera__translation_in_toolframe"])
R = self.bot.list_to_kdl_rotation(self.config["middle_camera_rotation_in_toolframe"])
self.tool_T_xe = PyKDL.Frame(R, P) # Fixed transform from middle eye frame to toolframe of head
self.xe_T_tool = self.tool_T_xe.Inverse()
#INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES INCLUDING, #BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; #LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER #CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT #LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN #ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE #POSSIBILITY OF SUCH DAMAGE. import m3.toolbox as m3t from m3qa.config_toolbox import * import copy # ############################################################## urdf_a2r2_right=m3t.get_m3_ros_config_path()+'defs/A2R2_r_arm_defs.urdf.xacro' #set MA* urdf_a2r2_right=m3t.get_m3_ros_config_path()+'defs/A2R2_l_arm_defs.urdf.xacro' #set MA* urdf_a2r2_right=m3t.get_m3_ros_config_path()+'robots/m3_A2R2_T2R2_torso_arms.urdf.xacro' #set MT* client=get_client_code() f=open(urdf_a2r2_right,'r') config_dir=m3t.get_m3_ros_config_path()+client try: os.mkdir(config_dir) print 'Made dir: ',config_dir except OSError: print 'Dir exists: ', config_dir fn_d=config_dir+d['name']+'.yml' f=file(fn_d,'w') print 'Saving...',fn_d f.write(yaml.safe_dump(d, default_flow_style=False,width=200))
#FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
#Copyright OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
#INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES INCLUDING,
#BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
#LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
#CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
#LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
#ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
#POSSIBILITY OF SUCH DAMAGE.
import m3.toolbox as m3t
from m3qa.config_toolbox import *
import copy
# ##############################################################
urdf_a2r2_right = m3t.get_m3_ros_config_path(
) + 'defs/A2R2_r_arm_defs.urdf.xacro' #set MA*
urdf_a2r2_right = m3t.get_m3_ros_config_path(
) + 'defs/A2R2_l_arm_defs.urdf.xacro' #set MA*
urdf_a2r2_right = m3t.get_m3_ros_config_path(
) + 'robots/m3_A2R2_T2R2_torso_arms.urdf.xacro' #set MT*
client = get_client_code()
f = open(urdf_a2r2_right, 'r')
config_dir = m3t.get_m3_ros_config_path() + client
try:
os.mkdir(config_dir)
print 'Made dir: ', config_dir
except OSError:
print 'Dir exists: ', config_dir
fn_d = config_dir + d['name'] + '.yml'
f = file(fn_d, 'w')