I am trying to constraint the center of mass CoM projection on the ground to stay inside the the pressure cone of a four legged robot. I wanted to Implement a polyhedral convex set using the half-space representation: {x| A x ≤ b} I tried to use
V = VPolytope(foot_pose_in_world)
halfspace_polyhedral = HPolyhedron(V)
the foot_pose_in_world
is a 4-by-2 array. I got this error message
QH6214 qhull input error: not enough points(2) to construct initial simplex (need 5)
I tried to add the center
full_hull_verticies = np.concatenate((foot_pose_in_world, center))
V = VPolytope(full_hull_verticies)
halfspace_polyhedral = HPolyhedron(V)
but I got this error message QH6214 qhull input error: not enough points(2) to construct initial simplex (need 6)
What is the correct way to add constraint for CoM to stay inside the pressure cone of the stance legs?
Thanks
I think you'll be happy to see that we have ComInPolyhedronConstraint available for you.
Of course you could make the approach you were trying above work, too. I believe you need to transpose the input. VPolytope (and all methods in Drake), wants the data to be in column-major format.
import numpy as np
from pydrake.geometry.optimization import VPolytope, HPolyhedron
x = np.array([[1,1],[-1,1],[1,-1],[-1,-1]])
V = VPolytope(x.T)
H = HPolyhedron(V)
print(H.A())
print(H.b())