Skip to content

Commit

Permalink
barriers: Rename compute_qp_inequality to ..._inequalities
Browse files Browse the repository at this point in the history
  • Loading branch information
stephane-caron committed Jul 26, 2024
1 parent 0880f96 commit 462839b
Show file tree
Hide file tree
Showing 4 changed files with 30 additions and 12 deletions.
2 changes: 1 addition & 1 deletion examples/barriers/arm_ur5.py
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@
)
configuration.integrate_inplace(velocity, dt)

G, h = pos_barrier.compute_qp_inequality(configuration, dt=dt)
G, h = pos_barrier.compute_qp_inequalities(configuration, dt=dt)
distance_to_manipulator = configuration.get_transform_frame_to_world(
"ee_link"
).translation[1]
Expand Down
2 changes: 1 addition & 1 deletion pink/barriers/barrier.py
Original file line number Diff line number Diff line change
Expand Up @@ -203,7 +203,7 @@ def compute_qp_objective(

return (H, c)

def compute_qp_inequality(
def compute_qp_inequalities(
self,
configuration: Configuration,
dt: float = 1e-3,
Expand Down
4 changes: 3 additions & 1 deletion pink/solve_ik.py
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,9 @@ def __compute_qp_inequalities(
G_list.append(matvec[0])
h_list.append(matvec[1])
for barrier in barriers:
G_barrier, h_barrier = barrier.compute_qp_inequality(configuration, dt)
G_barrier, h_barrier = barrier.compute_qp_inequalities(
configuration, dt
)
G_list.append(G_barrier)
h_list.append(h_barrier)
if not G_list:
Expand Down
34 changes: 25 additions & 9 deletions tests/test_barrier.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,9 @@ class TestBarrier(unittest.TestCase):
def setUp(self):
"""Set up test fixtures."""

self.robot = load_robot_description("upkie_description", root_joint=pin.JointModelFreeFlyer())
self.robot = load_robot_description(
"upkie_description", root_joint=pin.JointModelFreeFlyer()
)
self.conf = Configuration(
self.robot.model,
self.robot.data,
Expand All @@ -32,9 +34,11 @@ def setUp(self):

def test_diff_form_dimensions(self):
"""Velocity barrier dimension is the number of bounded joints."""
for barrier in [PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))]:
for barrier in [
PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))
]:
H, c = barrier.compute_qp_objective(self.conf)
G, h = barrier.compute_qp_inequality(self.conf, self.dt)
G, h = barrier.compute_qp_inequalities(self.conf, self.dt)
self.assertEqual(H.shape[0], self.robot.nv)
self.assertEqual(H.shape[1], self.robot.nv)
self.assertEqual(c.shape[0], self.robot.nv)
Expand All @@ -44,21 +48,27 @@ def test_diff_form_dimensions(self):
def test_barrier_value_dimension(self):
"""Test tha shape of value in all barriers is correct."""

for barrier in [PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))]:
for barrier in [
PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))
]:
v = barrier.compute_barrier(self.conf)
self.assertEqual(v.shape[0], barrier.dim)

def test_barrier_jacobians_dimension(self):
"""Test that shapes of jacobians in all barriers are correct."""

for barrier in [PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))]:
for barrier in [
PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))
]:
J = barrier.compute_jacobian(self.conf)
self.assertEqual(J.shape[0], barrier.dim)
self.assertEqual(J.shape[1], self.robot.nv)

def test_barrier_without_penalty_weight(self):
"""Test that objective is zero if no penalty weight is provided."""
for barrier in [PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))]:
for barrier in [
PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))
]:
H, c = barrier.compute_qp_objective(self.conf)
self.assertTrue(np.allclose(H, 0))
self.assertTrue(np.allclose(c, 0))
Expand All @@ -80,7 +90,11 @@ def test_barrier_penalty_weight(self):

def test_task_repr(self):
"""Test task string representation."""
for limit in [PositionBarrier("universe", safe_displacement_gain=0.0, p_min=np.zeros(3))]:
for limit in [
PositionBarrier(
"universe", safe_displacement_gain=0.0, p_min=np.zeros(3)
)
]:
self.assertTrue("gain=" in repr(limit))
self.assertTrue("safe_displacement=" in repr(limit))
self.assertTrue("safe_displacement_gain" in repr(limit))
Expand All @@ -89,12 +103,14 @@ def test_task_repr(self):
def test_cached(self):
"""Test that cached results are reused."""

barrier = PositionBarrier("left_hip", p_min=np.zeros(3), p_max=np.zeros(3))
barrier = PositionBarrier(
"left_hip", p_min=np.zeros(3), p_max=np.zeros(3)
)
barrier.compute_qp_objective(self.conf)
# Check that cache is initially triggered
self.assertIsNotNone(barrier._Barrier__q_cache)

# Check that cache is resetted after update
self.conf.update(self.conf.q + 1.0)
barrier.compute_qp_inequality(self.conf)
barrier.compute_qp_inequalities(self.conf)
self.assertTrue(np.allclose(barrier._Barrier__q_cache, self.conf.q))

0 comments on commit 462839b

Please sign in to comment.