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