def create_cc(opti, robot: Robot, scene: Scene, q):
poly_robot = []
for link in robot.links:
poly_robot.extend(link.geometry.get_polyhedrons())
if robot.geometry_tool is not None:
poly_robot.extend(robot.geometry_tool.get_polyhedrons())
poly_scene = scene.get_polyhedrons()
S = len(poly_robot[0].b)
nobs = len(poly_scene)
N, _ = q.shape
robs = len(poly_robot)
# dual variables arranged in convenient lists to acces with indices
lam = [
[[opti.variable(S) for j in range(nobs)] for i in range(robs)] for k in range(N)
]
mu = [
[[opti.variable(S) for j in range(nobs)] for i in range(robs)] for k in range(N)
]
cons = []
for k in range(N):
cons.extend(
create_cc_for_joint_pose(
robot, poly_robot, poly_scene, q[k, :], lam[k], mu[k]
)
)
return cons
def test_polyhedron(self):
b = Box(1, 2, 3)
tf = pose_z(0.3, 0.1, 0.2, -0.3)
col = Scene([b], [tf])
polys = col.get_polyhedrons()
assert len(polys) == 1
Aa = np.array(
[[1, 0, 0], [-1, 0, 0], [0, 1, 0], [0, -1, 0], [0, 0, 1], [0, 0, -1]]
)
ba = np.array([0.5, 0.5, 1, 1, 1.5, 1.5])
Aa = np.dot(Aa, tf[:3, :3].T)
ba = ba + np.dot(Aa, tf[:3, 3])
assert_almost_equal(polys[0].A, Aa)
assert_almost_equal(polys[0].b, ba)