Search code examples
drake

constraint the center of mass CoM projection on the ground to stay inside the the pressure cone


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


Solution

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