Ejemplo n.º 1
0
    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"],
            },
        }
Ejemplo n.º 2
0
    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()
Ejemplo n.º 3
0
    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']}}
Ejemplo n.º 4
0
    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()
Ejemplo n.º 5
0
#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))
Ejemplo n.º 6
0
#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')