def test_string(): handler = blf.StdParametersHandler() handler.set_parameter_string(name="my_string", value="foo") assert handler.get_parameter_string(name="my_string") == "foo" with pytest.raises(ValueError): handler.get_parameter_bool(name="my_string") with pytest.raises(ValueError): handler.get_parameter_int(name="my_string") with pytest.raises(ValueError): handler.get_parameter_float(name="my_string")
def test_bool(): handler = blf.StdParametersHandler() handler.set_parameter_bool(name="my_bool", value=True) assert handler.get_parameter_bool(name="my_bool") is True with pytest.raises(ValueError): handler.get_parameter_int(name="my_bool") with pytest.raises(ValueError): handler.get_parameter_float(name="my_bool") with pytest.raises(ValueError): handler.get_parameter_string(name="my_bool")
def test_float(): handler = blf.StdParametersHandler() handler.set_parameter_float(name="my_float", value=3.1415) assert handler.get_parameter_float( name="my_float") == pytest.approx(3.1415) with pytest.raises(ValueError): handler.get_parameter_bool(name="my_float") with pytest.raises(ValueError): handler.get_parameter_int(name="my_float") with pytest.raises(ValueError): handler.get_parameter_string(name="my_float")
def test_vector_string(): handler = blf.StdParametersHandler() handler.set_parameter_vector_string( name="my_vector_string", value=["foo", "bar", "bipedal", "locomotion"]) assert handler.get_parameter_vector_string(name="my_vector_string") == \ ["foo", "bar", "bipedal", "locomotion"] with pytest.raises(ValueError): handler.get_parameter_vector_bool(name="my_vector_string") with pytest.raises(ValueError): handler.get_parameter_vector_int(name="my_vector_string") with pytest.raises(ValueError): handler.get_parameter_vector_float(name="my_vector_string")
def test_vector_float(): handler = blf.StdParametersHandler() handler.set_parameter_vector_float(name="my_vector_float", value=[-3.14, 2.7182, 42.0]) assert handler.get_parameter_vector_float(name="my_vector_float") == \ pytest.approx([-3.14, 2.7182, 42.0]) with pytest.raises(ValueError): handler.get_parameter_vector_bool(name="my_vector_float") with pytest.raises(ValueError): handler.get_parameter_vector_int(name="my_vector_float") with pytest.raises(ValueError): handler.get_parameter_vector_string(name="my_vector_float")
def test_vector_int(): handler = blf.StdParametersHandler() handler.set_parameter_vector_int(name="my_vector_int", value=[-1, 2, 10]) assert handler.get_parameter_vector_int(name="my_vector_int") == [ -1, 2, 10 ] with pytest.raises(ValueError): handler.get_parameter_vector_bool(name="my_vector_int") with pytest.raises(ValueError): handler.get_parameter_vector_float(name="my_vector_int") with pytest.raises(ValueError): handler.get_parameter_vector_string(name="my_vector_int")
def test_vector_mixed(): handler = blf.StdParametersHandler() # 1. Mixed vector: store as more general type float handler.set_parameter_vector_float(name="to_float", value=[42.0, 1, -3.14, False]) assert handler.get_parameter_vector_float(name="to_float") == \ pytest.approx([42.0, 1.0, -3.14, 0.0]) # 2. Mixed vector: store as more general type int handler.set_parameter_vector_float(name="to_int", value=[42, 1, -3, False]) assert handler.get_parameter_vector_float(name="to_int") == \ pytest.approx([42, 1, -3, 0]) # 3. Mixed vector: store as less general type int with pytest.raises(TypeError): handler.set_parameter_vector_int(name="to_int_fail", value=[42.0, 1, -3.14, False])
def test_clear(): handler = blf.StdParametersHandler() handler.set_parameter_bool(name="my_bool1", value=False) handler.set_parameter_bool(name="my_bool2", value=True) handler.set_parameter_float(name="my_float", value=-42.42) handler.set_parameter_vector_string(name="my_vector_string", value=["bar", "foo"]) handler.clear() with pytest.raises(ValueError): _ = handler.get_parameter_bool(name="my_bool1") with pytest.raises(ValueError): _ = handler.get_parameter_bool(name="my_bool2") with pytest.raises(ValueError): _ = handler.get_parameter_float(name="my_float") with pytest.raises(ValueError): _ = handler.get_parameter_vector_string(name="my_float")
def test_schmitt_trigger_detector(): parameters_handler = blf.StdParametersHandler() parameters_handler.set_parameter_vector_string("contacts", ["right"]) parameters_handler.set_parameter_vector_float("contact_make_thresholds", [100.0]) parameters_handler.set_parameter_vector_float("contact_break_thresholds", [10.0]) parameters_handler.set_parameter_vector_float("contact_make_switch_times", [0.2]) parameters_handler.set_parameter_vector_float("contact_break_switch_times", [0.2, 0.2]) detector = blf.SchmittTriggerDetector() assert (detector.initialize(parameters_handler) == False) parameters_handler.set_parameter_vector_float("contact_break_switch_times", [0.2]) assert (detector.initialize(parameters_handler)) assert (detector.reset_contacts()) # rise signal assert (detector.set_timed_trigger_input("right", 0.1, 120.0)) assert (detector.advance()) assert (detector.set_timed_trigger_input("right", 0.2, 120.0)) assert (detector.advance()) assert (detector.set_timed_trigger_input("right", 0.3, 120.0)) assert (detector.advance()) # contact state should turn true right_contact = detector.get("right") assert (right_contact.is_active) assert (right_contact.switch_time == 0.3) # fall signal assert (detector.set_timed_trigger_input("right", 0.4, 7.0)) assert (detector.advance()) assert (detector.set_timed_trigger_input("right", 0.5, 7.0)) assert (detector.advance()) assert (detector.set_timed_trigger_input("right", 0.6, 7.0)) assert (detector.advance()) # contact state should turn false right_contact = detector.get("right") assert (right_contact.is_active == False) assert (right_contact.switch_time == 0.6) # add a new contact params = blf.SchmittTriggerParams() params.off_threshold = 10 params.on_threshold = 100 params.switch_off_after = 0.2 params.switch_on_after = 0.2 detector.add_contact("left", False, params, 0.6) contacts = detector.get() assert (len(contacts) == 2) assert (contacts["right"].is_active == False) # test multiple measurement updates right_input = blf.SchmittTriggerInput() right_input.time = 0.7 right_input.value = 120 left_input = blf.SchmittTriggerInput() left_input.time = 0.7 left_input.value = 120 timed_inputs = {"right": right_input, "left": left_input} assert (detector.set_timed_trigger_inputs(timed_inputs)) # test removing a contact assert (detector.remove_contact("left")) contacts = detector.get() assert (len(contacts) == 1) # test resetting a contact assert (detector.reset_contact("right", True, params)) right_contact = detector.get("right") assert (right_contact.is_active == True)
def test_legged_odometry(): # This function just performs an interface test for # the generated python bindings, # create KinDynComputationsDescriptor kindyn_handler = blf.StdParametersHandler() kindyn_handler.set_parameter_string("model_file_name", "./model.urdf") kindyn_handler.set_parameter_vector_string("joints_list", [ "neck_pitch", "neck_roll", "neck_yaw", "torso_pitch", "torso_roll", "torso_yaw", "l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", "r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", "r_elbow", "l_hip_pitch", "l_hip_roll", "l_hip_yaw", "l_knee", "l_ankle_pitch", "l_ankle_roll", "r_hip_pitch", "r_hip_roll", "r_hip_yaw", "r_knee", "r_ankle_pitch", "r_ankle_roll" ]) kindyn_desc = blf.construct_kindyncomputations_descriptor(kindyn_handler) assert kindyn_desc.is_valid() dt = 0.01 # create configuration parameters handler for legged odometry lo_params_handler = blf.StdParametersHandler() lo_params_handler.set_parameter_float("sampling_period_in_s", dt) model_info_group = blf.StdParametersHandler() model_info_group.set_parameter_string("base_link", "root_link") # use same frame for the IMU - requirement due to the software design model_info_group.set_parameter_string("base_link_imu", "root_link") # unused parameters but required for configuration (will be deprecated soon) model_info_group.set_parameter_string("left_foot_contact_frame", "l_foot") model_info_group.set_parameter_string("right_foot_contact_frame", "r_foot") assert (lo_params_handler.set_group("ModelInfo", model_info_group)) lo_group = blf.StdParametersHandler() lo_group.set_parameter_string("initial_fixed_frame", "l_sole") lo_group.set_parameter_string("initial_ref_frame_for_world", "l_sole") lo_group.set_parameter_vector_float( "initial_world_orientation_in_ref_frame", [1.0, 0.0, 0.0, 0.0]) lo_group.set_parameter_vector_float("initial_world_position_in_ref_frame", [0.0, 0.0, 0.0]) lo_group.set_parameter_string("switching_pattern", "useExternal") assert lo_params_handler.set_group("LeggedOdom", lo_group) # instantiate legged odometry legged_odom = blf.LeggedOdometry() empty_handler = blf.StdParametersHandler() # assert passing an empty parameter handler to false assert legged_odom.initialize(empty_handler, kindyn_desc.kindyn) == False # assert passing an properly configured parameter handler to true assert legged_odom.initialize(lo_params_handler, kindyn_desc.kindyn) == True # shape of the robot for the above specified joints list encoders = np.array([ -0.0001, 0.0000, 0.0000, 0.1570, 0.0003, -0.0000, -0.0609, 0.4350, 0.1833, 0.5375, -0.0609, 0.4349, 0.1834, 0.5375, 0.0895, 0.0090, -0.0027, -0.5694, -0.3771, -0.0211, 0.0896, 0.0090, -0.0027, -0.5695, -0.3771, -0.0211 ]) encoder_speeds = np.zeros_like(encoders) for time in np.arange(start=0, step=dt, stop=10 * dt): fixed_frame_idx = legged_odom.get_fixed_frame_index() # here we only fill measurement buffers assert (legged_odom.set_kinematics(encoders, encoder_speeds)) contact_name = "l_sole" contact_status = True switch_time = time time_now = time assert legged_odom.set_contact_status(contact_name, contact_status, switch_time, time_now) # since we set "switching_pattern" parameter to "useExternal" # we can change the fixed frame manually from an external script # such as this script, where for example, we change the same # frame as we had earlier initialized the estimator # if the "switching_pattern" is not set to `useExternal` # this function call will return false legged_odom.change_fixed_frame(fixed_frame_idx) # computations given the set measurements assert legged_odom.advance() # sample asserts for show-casing outputs out = legged_odom.get() assert len(out.base_twist) == 6 assert len(out.state.imu_linear_velocity) == 3
def test_time_varying_dcm_planner(): # Create the map of contact lists contact_list_map = dict() # Left foot contact_list_map["left"] = blf.ContactList() # L1: first footstep assert contact_list_map["left"].add_contact(transform=blf.SE3( position=np.array([0, -0.8, 0]), quaternion=np.array([0, 0, 0, 1])), activation_time=0.0, deactivation_time=1.0) # L2: second footstep assert contact_list_map["left"].add_contact(transform=blf.SE3( position=np.array([0.25, -0.8, 0.2]), quaternion=np.array([0, 0, 0, 1])), activation_time=2.0, deactivation_time=7.0) # Right foot contact_list_map["right"] = blf.ContactList() # R1: first footstep assert contact_list_map["right"].add_contact(transform=blf.SE3( position=np.array([0, 0.8, 0]), quaternion=np.array([0, 0, 0, 1])), activation_time=0.0, deactivation_time=3.0) # R2: second footstep assert contact_list_map["right"].add_contact( transform=blf.SE3(position=np.array([0.25, 0.8, 0.2]), quaternion=np.array([0, 0, 0, 1])), activation_time=4.0, deactivation_time=7.0) # Create the contact phase list phase_list = blf.ContactPhaseList() phase_list.set_lists(contact_list_map) # Set the parameters handler = blf.StdParametersHandler() handler.set_parameter_float(name="planner_sampling_time", value=0.05) handler.set_parameter_int(name="number_of_foot_corners", value=4) # Set the foot corners handler.set_parameter_vector_float(name="foot_corner_0", value=[0.1, 0.05, 0.0]) handler.set_parameter_vector_float(name="foot_corner_1", value=[0.1, -0.05, 0.0]) handler.set_parameter_vector_float(name="foot_corner_2", value=[-0.1, -0.05, 0.0]) handler.set_parameter_vector_float(name="foot_corner_3", value=[-0.1, 0.05, 0.0]) # Set the weight of the cost function handler.set_parameter_float("omega_dot_weight", 1.0) handler.set_parameter_float("dcm_tracking_weight", 1.0) handler.set_parameter_float("omega_dot_rate_of_change_weight", 10.0) handler.set_parameter_float("vrp_rate_of_change_weight", 100.0) handler.set_parameter_float("dcm_rate_of_change_weight", 1.0) # Set the initial state initial_state = blf.DCMPlannerState() initial_state.dcm_position = np.array([0, 0, 0.53]) initial_state.dcm_velocity = np.zeros(3) initial_state.vrp_position = initial_state.dcm_position initial_state.omega = np.sqrt(9.81 / initial_state.dcm_position[2]) # Initialize the planner planner = blf.TimeVaryingDCMPlanner() assert planner.initialize(handler=handler) assert planner.set_contact_phase_list(contact_phase_list=phase_list) planner.set_initial_state(state=initial_state) assert planner.compute_trajectory() for _ in range(150): assert planner.advance()
def test_swing_foot_planner(): contact_list = blf.ContactList() from scipy.spatial.transform import Rotation as R # First footstep tf1 = blf.SE3(position=[0.9, 0, 0], quaternion=R.from_euler(seq="z", angles=np.pi / 2).as_quat()) assert contact_list.add_contact(transform=tf1, activation_time=0.0, deactivation_time=0.3) # Second footstep tf2 = blf.SE3(position=[0.8899, 0.1345, 0.0600], quaternion=R.from_euler(seq="z", angles=1.7208).as_quat()) assert contact_list.add_contact(transform=tf2, activation_time=0.6, deactivation_time=1.5) # Third footstep tf3 = blf.SE3(position=[0.8104, 0.3915, 0.1800], quaternion=R.from_euler(seq="z", angles=2.0208).as_quat()) assert contact_list.add_contact(transform=tf3, activation_time=1.8, deactivation_time=2.7) # Fourth footstep tf4 = blf.SE3(position=[0.6585, 0.6135, 0.3000], quaternion=R.from_euler(seq="z", angles=2.3208).as_quat()) assert contact_list.add_contact(transform=tf4, activation_time=3.0, deactivation_time=3.9) # Fifth footstep tf5 = blf.SE3(position=[0.3261, 0.8388, 0.4800], quaternion=R.from_euler(seq="z", angles=2.7708).as_quat()) assert contact_list.add_contact(transform=tf5, activation_time=4.2, deactivation_time=6.0) dT = 0.01 parameters_handler = blf.StdParametersHandler() parameters_handler.set_parameter_float("sampling_time", dT) parameters_handler.set_parameter_float("step_height", 0.1) parameters_handler.set_parameter_float("foot_apex_time", 0.5) parameters_handler.set_parameter_float("foot_landing_velocity", 0.0) parameters_handler.set_parameter_float("foot_landing_acceleration", 0.0) planner = blf.SwingFootPlanner() assert planner.initialize(handler=parameters_handler) planner.set_contact_list(contact_list=contact_list) num_of_iterations = ( contact_list[len(contact_list) - 1].deactivation_time + 1) / dT for i in range(int(num_of_iterations)): # state = planner.get() # print(state.transform[0:3], state.transform[3:]) assert planner.advance()