Example #1
0
 def test_integrate(self):
     fk = Forward_kinematics('Duty_fi_linear_no_constant',
                             'Duty_fi_linear_no_constant',
                             np.matrix([-1, 1]), np.matrix([1, 1]))
     theta_dot = np.pi / 2
     v = np.pi / 2
     dt = 1
     theta_res, x_res, y_res = fk.integrate(theta_dot, v, dt)
     self.assertAlmostEqual(theta_res, np.pi / 2)
     self.assertAlmostEqual(x_res, 1)
     self.assertAlmostEqual(y_res, 1)
Example #2
0
 def test_zero_theta_dot(self):
     fk = Forward_kinematics('Duty_fi_linear_no_constant',
                             'Duty_fi_linear_no_constant',
                             np.matrix([-1, 1]), np.matrix([1, 1]))
     theta, x, y = 0, 0, 0
     theta_dot = 0
     v = 1
     dt = 1
     theta_res, x_res, y_res = fk.integrate_propagate(
         theta, x, y, theta_dot, v, dt)
     self.assertAlmostEqual(theta_res, 0)
     self.assertAlmostEqual(x_res, 1)
     self.assertAlmostEqual(y_res, 0)
Example #3
0
    def __init__(self):
        self.node_name = 'position_filter_node'

        # Read parameters
        self.veh_name = self.setupParameter("~veh_name", "megaman")

        fi_theta_dot_function = self.setupParameter(
            '~fi_theta_dot_function_param', 'Duty_fi_theta_dot_naive')
        fi_v_function = self.setupParameter('~fi_v_function_param',
                                            'Duty_fi_v_naive')
        theta_dot_weights = matrix(
            self.setupParameter('~theta_dot_weights_param', [-1.0]))
        v_weights = matrix(self.setupParameter('~v_weights_param', [1.0]))

        #Setup the forward kinematics model
        self.fk = Forward_kinematics.Forward_kinematics(
            fi_theta_dot_function, fi_v_function, matrix(theta_dot_weights),
            matrix(v_weights))

        #Setup the publisher and subscriber
        self.sub_velocity = rospy.Subscriber("~velocity", Twist2DStamped,
                                             self.velocityCallback)
        self.pub_pose = rospy.Publisher("~pose", Pose2DStamped, queue_size=1)

        #Keep track of the last known pose
        self.last_pose = Pose2DStamped()
        self.last_theta_dot = 0
        self.last_v = 0

        rospy.loginfo("[%s] has started", self.node_name)
    def __init__(self):
        self.node_name = 'forward_kinematics_node'

        # Read parameters
        # self.veh_name = self.setupParameter("~veh_name","megaman")
        fi_theta_dot_function = self.setupParameter('~fi_theta_dot_function_param', 'Duty_fi_theta_dot_naive')
        fi_v_function = self.setupParameter('~fi_v_function_param', 'Duty_fi_v_naive')
        theta_dot_weights = matrix(self.setupParameter('~theta_dot_weights_param', [-1.0]))
        v_weights = matrix(self.setupParameter('~v_weights_param', [1.0]))
        self.filename = self.setupParameter("~FIfile","FIfile")

        #Setup the forward kinematics model
        self.fk = Forward_kinematics.Forward_kinematics(fi_theta_dot_function, fi_v_function, theta_dot_weights, v_weights)

        #Setup the publisher and subscribers
        self.pub_velocity = rospy.Publisher("~velocity", Twist2DStamped, queue_size=1)
        self.sub = rospy.Subscriber("~wheels_cmd", WheelsCmdStamped, self.wheelsCmdCallback)
        self.sub_theta_dot_weights = rospy.Subscriber("~theta_dot_weights", KinematicsWeights, self.thedaDotWeightsCallback)
        self.sub_v_weights = rospy.Subscriber("~v_weights", KinematicsWeights, self.vWeightsCallback)

        rospy.loginfo("[%s] has started", self.node_name)
Example #5
0
    def test_with_naive_fi(self):
        fk = Forward_kinematics('Duty_fi_theta_dot_naive', 'Duty_fi_v_naive',
                                np.matrix([-1]), np.matrix([1]))

        theta_dot, v = fk.evaluate(np.matrix([0]), np.matrix([0]))
        self.assertAlmostEqual(theta_dot, 0)
        self.assertAlmostEqual(v, 0)

        theta_dot, v = fk.evaluate(np.matrix([1]), np.matrix([1]))
        self.assertAlmostEqual(theta_dot, 0)
        self.assertAlmostEqual(v, 2)

        theta_dot, v = fk.evaluate(np.matrix([1]), np.matrix([-1]))
        self.assertAlmostEqual(theta_dot, -2)
        self.assertAlmostEqual(v, 0)

        theta_dot, v = fk.evaluate(np.matrix([-1]), np.matrix([1]))
        self.assertAlmostEqual(theta_dot, 2)
        self.assertAlmostEqual(v, 0)
Example #6
0
    def test_with_linear_fi(self):
        fk = Forward_kinematics('Duty_fi_linear_no_constant',
                                'Duty_fi_linear_no_constant',
                                np.matrix([-1, 1]), np.matrix([1, 1]))

        theta_dot, v = fk.evaluate(np.matrix([0]), np.matrix([0]))
        self.assertAlmostEqual(theta_dot, 0)
        self.assertAlmostEqual(v, 0)

        theta_dot, v = fk.evaluate(np.matrix([1]), np.matrix([1]))
        self.assertAlmostEqual(theta_dot, 0)
        self.assertAlmostEqual(v, 2)

        theta_dot, v = fk.evaluate(np.matrix([1]), np.matrix([-1]))
        self.assertAlmostEqual(theta_dot, -2)
        self.assertAlmostEqual(v, 0)

        theta_dot, v = fk.evaluate(np.matrix([-1]), np.matrix([1]))
        self.assertAlmostEqual(theta_dot, 2)
        self.assertAlmostEqual(v, 0)

        theta_dot, v = fk.evaluate(np.matrix([0]), np.matrix([2]))
        self.assertAlmostEqual(theta_dot, 2)
        self.assertAlmostEqual(v, 2)