コード例 #1
0
    def setUp(self):



        #config_filename = "config/system.yaml"
        #f = open(config_filename)
        f = rospy.get_param("system")
        all_config = yaml.load(f)

        self.robot_params = RobotParams()
        self.robot_params.configure(all_config)


        rospy.wait_for_service('fk', 3.0)
        self._fk_ref = rospy.ServiceProxy('fk', FkTest)
コード例 #2
0
class LoadData(unittest.TestCase):
    def setUp(self):



        #config_filename = "config/system.yaml"
        #f = open(config_filename)
        f = rospy.get_param("system")
        all_config = yaml.load(f)

        self.robot_params = RobotParams()
        self.robot_params.configure(all_config)


        rospy.wait_for_service('fk', 3.0)
        self._fk_ref = rospy.ServiceProxy('fk', FkTest)
        #f.close()

    def loadCommands(self, param_name):
        #f = open(filename)
        #cmds = [ [ float(y) for y in x.split()] for x in f.readlines()]
        #f.close()

        command_str = rospy.get_param(param_name)


        cmds = [ [float(y) for y in x.split()] for x in command_str.strip().split('\n')]

        return cmds

    def getExpected(self, root, tip, cmd):
        resp = self._fk_ref(root, tip, cmd)
        #print resp
        T = matrix(zeros((4,4)), float)
        T[0:3, 0:3] = reshape( matrix(resp.rot, float), (3,3))
        T[3,3] = 1.0;
        T[0:3,3] = reshape( matrix(resp.pos, float), (3,1))
        return T
コード例 #3
0
    def test_single_sensor(self):
        config = yaml.load('''
                chain_id: chainA
                before_chain: [transformA]
                dh_link_num:  1
                after_chain:  [transformB]
            ''')

        robot_params = RobotParams()
        robot_params.configure( yaml.load('''
            dh_chains:
              chainA:
              - [ 0, 0, 1, 0 ]
              chainB:
              - [ 0, 0, 2, 0 ]
            tilting_lasers: {}
            rectified_cams: {}
            transforms:
                transformA: [0, 0, 0, 0, 0, 0]
                transformB: [0, 0, 0, 0, 0, 0]
            checkerboards:
              boardA:
                corners_x: 2
                corners_y: 2
                spacing_x: 1
                spacing_y: 1
            ''' ) )

        free_dict = yaml.load('''
            dh_chains:
              chainA:
              - [ 1, 0, 1, 0 ]
              chainB:
              - [ 0, 0, 0, 0 ]
            tilting_lasers: {}
            rectified_cams: {}
            transforms:
                transformA: [1, 0, 0, 0, 0, 0]
                transformB: [0, 0, 0, 0, 0, 0]
            checkerboards:
              boardA:
                spacing_x: 1
                spacing_y: 1
            ''')

        chainM = ChainMeasurement( chain_id="chainA", chain_state=JointState(position=[0]))

        sensor = ChainSensor(config, chainM, "boardA" )
        sensor.update_config(robot_params)

        target_pts = matrix([[1, 2, 1, 2],
                             [0, 0, 1, 1],
                             [0, 0, 0, 0],
                             [1, 1, 1, 1]])

        calc = ErrorCalc(robot_params, free_dict, [])

        expanded_param_vec = robot_params.deflate()
        free_list = robot_params.calc_free(free_dict)
        opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
        opt_param = numpy.array(opt_param_mat)[0]

        J = calc.single_sensor_params_jacobian(opt_param, target_pts, sensor)

        print J
コード例 #4
0
    def test_full_jacobian(self):
        configA = yaml.load('''
                chain_id: chainA
                before_chain: [transformA]
                dh_link_num:  1
                after_chain:  []
            ''')

        configB = yaml.load('''
                chain_id: chainB
                before_chain: []
                dh_link_num:  1
                after_chain:  [transformB]
            ''')

        robot_params = RobotParams()
        robot_params.configure( yaml.load('''
            dh_chains:
              chainA:
              - [ 0, 0, 1, 0 ]
              chainB:
              - [ 0, 0, 2, 0 ]
            tilting_lasers:
              laserB:
                before_joint: [0, 0, 0, 0, 0, 0]
                after_joint:  [0, 0, 0, 0, 0, 0]
            rectified_cams: {}
            transforms:
                transformA: [0, 0, 0, 0, 0, 0]
                transformB: [0, 0, 0, 0, 0, 0]
            checkerboards:
              boardA:
                corners_x: 2
                corners_y: 2
                spacing_x: 1
                spacing_y: 1
              boardB:
                corners_x: 1
                corners_y: 1
                spacing_x: 1
                spacing_y: 1
            ''' ) )

        free_dict = yaml.load('''
            dh_chains:
              chainA:
              - [ 1, 0, 0, 0 ]
              chainB:
              - [ 0, 0, 0, 0 ]
            tilting_lasers:
              laserB:
                before_joint: [1, 0, 0, 0, 0, 0]
                after_joint:  [0, 0, 0, 0, 0, 0]
            rectified_cams: {}
            transforms:
                transformA: [0, 0, 0, 0, 0, 0]
                transformB: [1, 0, 0, 0, 0, 0]
            checkerboards:
              boardA:
                spacing_x: 1
                spacing_y: 1
              boardB:
                spacing_x: 0
                spacing_y: 0
            ''')

        sensorA = ChainSensor(configA, ChainMeasurement( chain_id="chainA", chain_state=JointState(position=[0])), "boardA")
        sensorB = ChainSensor(configB, ChainMeasurement( chain_id="chainB", chain_state=JointState(position=[0])), "boardB")
        laserB  = TiltingLaserSensor({'laser_id':'laserB'}, LaserMeasurement( laser_id="laserB",
                                                                              joint_points=[ JointState(position=[0,0,2]) ] ) )

        multisensorA = MultiSensor(None)
        multisensorA.sensors = [sensorA]
        multisensorA.checkerboard = "boardA"
        multisensorA.update_config(robot_params)

        multisensorB = MultiSensor(None)
        multisensorB.sensors = [sensorB, laserB]
        multisensorB.checkerboard = "boardB"
        multisensorB.update_config(robot_params)

        poseA = numpy.array([1, 0, 0, 0, 0, 0])
        poseB = numpy.array([2, 0, 0, 0, 0, 0])

        calc = ErrorCalc(robot_params, free_dict, [multisensorA, multisensorB])

        expanded_param_vec = robot_params.deflate()
        free_list = robot_params.calc_free(free_dict)
        opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
        opt_param = numpy.array(opt_param_mat)[0]
        opt_all_vec = concatenate([opt_param, poseA, poseB])

        print "Calculating Sparse"
        J = calc.calculate_jacobian(opt_all_vec)
        numpy.savetxt("J_sparse.txt", J, fmt="% 4.3f")

        #import code; code.interact(local=locals())

        print "Calculating Dense"
        from scipy.optimize.slsqp import approx_jacobian
        J_naive = approx_jacobian(opt_all_vec, calc.calculate_error, 1e-6)
        numpy.savetxt("J_naive.txt", J_naive, fmt="% 4.3f")


        self.assertAlmostEqual(numpy.linalg.norm(J-J_naive), 0.0, 6)
コード例 #5
0
    def test_multisensor_pose_jacobian(self):
        configA = yaml.load('''
                chain_id: chainA
                before_chain: [transformA]
                dh_link_num:  1
                after_chain:  [transformB]
            ''')

        configB = yaml.load('''
                chain_id: chainB
                before_chain: [transformA]
                dh_link_num:  1
                after_chain:  [transformB]
            ''')

        robot_params = RobotParams()
        robot_params.configure( yaml.load('''
            dh_chains:
              chainA:
              - [ 0, 0, 1, 0 ]
              chainB:
              - [ 0, 0, 2, 0 ]
            tilting_lasers: {}
            rectified_cams: {}
            transforms:
                transformA: [0, 0, 0, 0, 0, 0]
                transformB: [0, 0, 0, 0, 0, 0]
            checkerboards:
              boardA:
                corners_x: 2
                corners_y: 2
                spacing_x: 1
                spacing_y: 1
            ''' ) )

        free_dict = yaml.load('''
            dh_chains:
              chainA:
              - [ 1, 0, 0, 0 ]
              chainB:
              - [ 0, 0, 0, 0 ]
            tilting_lasers: {}
            rectified_cams: {}
            transforms:
                transformA: [0, 0, 0, 0, 0, 0]
                transformB: [0, 0, 0, 0, 0, 0]
            checkerboards:
              boardA:
                spacing_x: 0
                spacing_y: 0
            ''')

        sensorA = ChainSensor(configA, ChainMeasurement( chain_id="chainA", chain_state=JointState(position=[0])), "boardA")

        sensorB = ChainSensor(configB, ChainMeasurement( chain_id="chainB", chain_state=JointState(position=[0])), "boardA")

        multisensor = MultiSensor(None)
        multisensor.sensors = [sensorA, sensorB]
        multisensor.checkerboard = "boardA"

        pose_param_vec = numpy.array([0, 0, 0, 0, 0, 0])
        calc = ErrorCalc(robot_params, free_dict, [])

        expanded_param_vec = robot_params.deflate()
        free_list = robot_params.calc_free(free_dict)
        opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
        opt_param = numpy.array(opt_param_mat)[0]

        J = calc.multisensor_pose_jacobian(opt_param, pose_param_vec, multisensor)

        print J
コード例 #6
0
    def test_single_sensor(self):
        config = yaml.load('''
                chain_id: chainA
                before_chain: [transformA]
                dh_link_num:  1
                after_chain:  [transformB]
            ''')

        robot_params = RobotParams()
        robot_params.configure(
            yaml.load('''
            dh_chains:
              chainA:
              - [ 0, 0, 1, 0 ]
              chainB:
              - [ 0, 0, 2, 0 ]
            tilting_lasers: {}
            rectified_cams: {}
            transforms:
                transformA: [0, 0, 0, 0, 0, 0]
                transformB: [0, 0, 0, 0, 0, 0]
            checkerboards:
              boardA:
                corners_x: 2
                corners_y: 2
                spacing_x: 1
                spacing_y: 1
            '''))

        free_dict = yaml.load('''
            dh_chains:
              chainA:
              - [ 1, 0, 1, 0 ]
              chainB:
              - [ 0, 0, 0, 0 ]
            tilting_lasers: {}
            rectified_cams: {}
            transforms:
                transformA: [1, 0, 0, 0, 0, 0]
                transformB: [0, 0, 0, 0, 0, 0]
            checkerboards:
              boardA:
                spacing_x: 1
                spacing_y: 1
            ''')

        chainM = ChainMeasurement(chain_id="chainA",
                                  chain_state=JointState(position=[0]))

        sensor = ChainSensor(config, chainM, "boardA")
        sensor.update_config(robot_params)

        target_pts = matrix([[1, 2, 1, 2], [0, 0, 1, 1], [0, 0, 0, 0],
                             [1, 1, 1, 1]])

        calc = ErrorCalc(robot_params, free_dict, [])

        expanded_param_vec = robot_params.deflate()
        free_list = robot_params.calc_free(free_dict)
        opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
        opt_param = numpy.array(opt_param_mat)[0]

        J = calc.single_sensor_params_jacobian(opt_param, target_pts, sensor)

        print J
コード例 #7
0
    def test_full_jacobian(self):
        configA = yaml.load('''
                chain_id: chainA
                before_chain: [transformA]
                dh_link_num:  1
                after_chain:  []
            ''')

        configB = yaml.load('''
                chain_id: chainB
                before_chain: []
                dh_link_num:  1
                after_chain:  [transformB]
            ''')

        robot_params = RobotParams()
        robot_params.configure(
            yaml.load('''
            dh_chains:
              chainA:
              - [ 0, 0, 1, 0 ]
              chainB:
              - [ 0, 0, 2, 0 ]
            tilting_lasers:
              laserB:
                before_joint: [0, 0, 0, 0, 0, 0]
                after_joint:  [0, 0, 0, 0, 0, 0]
            rectified_cams: {}
            transforms:
                transformA: [0, 0, 0, 0, 0, 0]
                transformB: [0, 0, 0, 0, 0, 0]
            checkerboards:
              boardA:
                corners_x: 2
                corners_y: 2
                spacing_x: 1
                spacing_y: 1
              boardB:
                corners_x: 1
                corners_y: 1
                spacing_x: 1
                spacing_y: 1
            '''))

        free_dict = yaml.load('''
            dh_chains:
              chainA:
              - [ 1, 0, 0, 0 ]
              chainB:
              - [ 0, 0, 0, 0 ]
            tilting_lasers:
              laserB:
                before_joint: [1, 0, 0, 0, 0, 0]
                after_joint:  [0, 0, 0, 0, 0, 0]
            rectified_cams: {}
            transforms:
                transformA: [0, 0, 0, 0, 0, 0]
                transformB: [1, 0, 0, 0, 0, 0]
            checkerboards:
              boardA:
                spacing_x: 1
                spacing_y: 1
              boardB:
                spacing_x: 0
                spacing_y: 0
            ''')

        sensorA = ChainSensor(
            configA,
            ChainMeasurement(chain_id="chainA",
                             chain_state=JointState(position=[0])), "boardA")
        sensorB = ChainSensor(
            configB,
            ChainMeasurement(chain_id="chainB",
                             chain_state=JointState(position=[0])), "boardB")
        laserB = TiltingLaserSensor(
            {'laser_id': 'laserB'},
            LaserMeasurement(laser_id="laserB",
                             joint_points=[JointState(position=[0, 0, 2])]))

        multisensorA = MultiSensor(None)
        multisensorA.sensors = [sensorA]
        multisensorA.checkerboard = "boardA"
        multisensorA.update_config(robot_params)

        multisensorB = MultiSensor(None)
        multisensorB.sensors = [sensorB, laserB]
        multisensorB.checkerboard = "boardB"
        multisensorB.update_config(robot_params)

        poseA = numpy.array([1, 0, 0, 0, 0, 0])
        poseB = numpy.array([2, 0, 0, 0, 0, 0])

        calc = ErrorCalc(robot_params, free_dict, [multisensorA, multisensorB])

        expanded_param_vec = robot_params.deflate()
        free_list = robot_params.calc_free(free_dict)
        opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
        opt_param = numpy.array(opt_param_mat)[0]
        opt_all_vec = concatenate([opt_param, poseA, poseB])

        print "Calculating Sparse"
        J = calc.calculate_jacobian(opt_all_vec)
        numpy.savetxt("J_sparse.txt", J, fmt="% 4.3f")

        #import code; code.interact(local=locals())

        print "Calculating Dense"
        from scipy.optimize.slsqp import approx_jacobian
        J_naive = approx_jacobian(opt_all_vec, calc.calculate_error, 1e-6)
        numpy.savetxt("J_naive.txt", J_naive, fmt="% 4.3f")

        self.assertAlmostEqual(numpy.linalg.norm(J - J_naive), 0.0, 6)
コード例 #8
0
    def test_multisensor_pose_jacobian(self):
        configA = yaml.load('''
                chain_id: chainA
                before_chain: [transformA]
                dh_link_num:  1
                after_chain:  [transformB]
            ''')

        configB = yaml.load('''
                chain_id: chainB
                before_chain: [transformA]
                dh_link_num:  1
                after_chain:  [transformB]
            ''')

        robot_params = RobotParams()
        robot_params.configure(
            yaml.load('''
            dh_chains:
              chainA:
              - [ 0, 0, 1, 0 ]
              chainB:
              - [ 0, 0, 2, 0 ]
            tilting_lasers: {}
            rectified_cams: {}
            transforms:
                transformA: [0, 0, 0, 0, 0, 0]
                transformB: [0, 0, 0, 0, 0, 0]
            checkerboards:
              boardA:
                corners_x: 2
                corners_y: 2
                spacing_x: 1
                spacing_y: 1
            '''))

        free_dict = yaml.load('''
            dh_chains:
              chainA:
              - [ 1, 0, 0, 0 ]
              chainB:
              - [ 0, 0, 0, 0 ]
            tilting_lasers: {}
            rectified_cams: {}
            transforms:
                transformA: [0, 0, 0, 0, 0, 0]
                transformB: [0, 0, 0, 0, 0, 0]
            checkerboards:
              boardA:
                spacing_x: 0
                spacing_y: 0
            ''')

        sensorA = ChainSensor(
            configA,
            ChainMeasurement(chain_id="chainA",
                             chain_state=JointState(position=[0])), "boardA")

        sensorB = ChainSensor(
            configB,
            ChainMeasurement(chain_id="chainB",
                             chain_state=JointState(position=[0])), "boardA")

        multisensor = MultiSensor(None)
        multisensor.sensors = [sensorA, sensorB]
        multisensor.checkerboard = "boardA"

        pose_param_vec = numpy.array([0, 0, 0, 0, 0, 0])
        calc = ErrorCalc(robot_params, free_dict, [])

        expanded_param_vec = robot_params.deflate()
        free_list = robot_params.calc_free(free_dict)
        opt_param_mat = expanded_param_vec[numpy.where(free_list), 0].copy()
        opt_param = numpy.array(opt_param_mat)[0]

        J = calc.multisensor_pose_jacobian(opt_param, pose_param_vec,
                                           multisensor)

        print J