def test_contact():
contact = blf.PlannedContact()
# Default values
assert contact.pose == blf.SE3(position=[0., 0, 0], quaternion=[0, 0, 0, 1])
assert contact.activation_time == 0.0
assert contact.deactivation_time == 0.0
assert contact.name == "Contact"
assert contact.type == blf.ContactType.Full
contact.pose = blf.SE3(position=[1.0, -2.0, 3.3], quaternion=[0, 0, -1, 0])
assert contact.pose.position == pytest.approx(np.array([1.0, -2.0, 3.3]))
assert contact.pose.quaternion == pytest.approx(np.array([0, 0, -1, 0]))
contact.activation_time = 42.0
assert contact.activation_time == 42.0
contact.deactivation_time = -42.0
assert contact.deactivation_time == -42.0
contact.name = "MyNewContact"
assert contact.name == "MyNewContact"
contact.type = blf.ContactType.Point
assert contact.type == blf.ContactType.Point
def test_se3():
# Test the default constructor
transform_default = blf.SE3()
assert transform_default.position == pytest.approx(np.array([0, 0, 0]))
assert transform_default.quaternion == pytest.approx(np.array([0, 0, 0,
1]))
# Test the custom constructor
transform = blf.SE3(position=np.array([0, 1, 2.]),
quaternion=np.array([0, 1., 0, 0.]))
assert transform.position == pytest.approx([0, 1, 2])
assert transform.quaternion == pytest.approx([0, 1., 0, 0.])
# Test the position setter
transform.position = np.array([-1, 42.0, 3.14])
assert transform.position == pytest.approx([-1, 42.0, 3.14])
# Create a quaternion
quaternion_not_normalized = np.array([1, 1, 0, 0])
quaternion = quaternion_not_normalized / np.linalg.norm(
quaternion_not_normalized)
# Test the quaternion setter
transform.quaternion = quaternion
assert transform.quaternion == pytest.approx(quaternion)
# Test equality operator
assert transform == blf.SE3(position=[-1, 42.0, 3.14],
quaternion=quaternion)
def test_swing_foot_planner_state():
state = blf.SwingFootPlannerState()
assert state.is_in_contact == True
state.is_in_contact = False
assert state.is_in_contact == False
state.transform = blf.SE3(position=np.array([0.0, -0.2, 0.5]),
quaternion=np.array([0, 0, 0, 1]))
assert state.transform.position == pytest.approx([0.0, -0.2, 0.5])
assert state.transform.quaternion == pytest.approx([0, 0, 0, 1])
state.mixed_velocity = np.array([0.1, 0.2, 0.3, -0.1, -0.2, -0.3])
assert state.mixed_velocity == pytest.approx(
[0.1, 0.2, 0.3, -0.1, -0.2, -0.3])
state.mixed_acceleration = np.array([0, 0.02, 0.03, -0.01, -0.02, -0.03])
assert state.mixed_acceleration == pytest.approx(
[0, 0.02, 0.03, -0.01, -0.02, -0.03])
def test_contact_list():
contact_list = blf.ContactList()
assert len(contact_list) == contact_list.size() == 0
assert contact_list.default_name() == "ContactList"
assert contact_list.default_contact_type() == blf.ContactType.Full
contact_list.set_default_name(default_name="MyContactList")
assert contact_list.default_name() == "MyContactList"
contact_list.set_default_contact_type(type=blf.ContactType.Point)
assert contact_list.default_contact_type() == blf.ContactType.Point
contact1 = blf.PlannedContact()
contact1.name = "Contact1"
contact1.activation_time = 0.1
contact1.deactivation_time = 0.5
contact2 = blf.PlannedContact()
contact2.name = "Contact2"
contact2.activation_time = 1.0
contact2.deactivation_time = 1.5
assert contact_list.add_contact(contact1)
assert contact_list.add_contact(contact2)
assert contact_list[0] == contact1
assert contact_list[len(contact_list) - 1] == contact2 # TODO: improve
contact3 = blf.PlannedContact()
contact3.name = "Contact3"
contact3.activation_time = 0.6
contact3.deactivation_time = 0.8
assert contact_list.add_contact(contact3)
assert len(contact_list) == 3
assert contact_list[1] == contact3
contact3_bis = blf.PlannedContact()
contact3_bis.name = "Contact3"
contact3_bis.activation_time = 0.9
contact3_bis.deactivation_time = 1.6
assert not contact_list.add_contact(contact3_bis)
contact2_modified = blf.PlannedContact()
contact2_modified.name = "Contact2Modified"
contact2_modified.type = blf.ContactType.Point
# TODO
# contact_list[len(contact_list) - 1] = contact2_modified
# assert contact_list[len(contact_list) - 1] == contact2_modified
assert contact2 == contact_list.get_present_contact(time=1.2)
assert contact2 == contact_list.get_present_contact(time=1.6)
assert contact3 == contact_list.get_present_contact(time=0.6)
# assert contact_list.get_present_contact(time=0.0) == \
# contact_list[len(contact_list) - 1]
contact_list.clear()
assert len(contact_list) == contact_list.size() == 0
for i in range(50):
assert contact_list.add_contact(
transform=blf.SE3(position=np.array([0, 0, 0]),
quaternion=np.array([0, 0, 0, 1.])),
activation_time=2.0 + i,
deactivation_time=2.5 + i)
assert len(contact_list) == contact_list.size() == 50
for idx, contact in enumerate(contact_list):
assert contact == contact_list[idx]
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()