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