I'm wondering if there are equivalents to numpy.sign() and numpy.clip() functions that can work with drake variables? For example the following code gives me the error:
from pydrake.all import Variable
q = Variable('q')
u = 1 + np.sign(q) # Doesn't work
u = 1 + np.clip(q, -1, 1) # Doesn't work
# Pass u into the dynamics of a SymbolicVectorSystem
RuntimeError: You should not call
__bool__/__nonzero__onFormula. If you are trying to make a map withVariable,Expression, orPolynomialas keys (and then access the map in Python), please use pydrake.common.containers.EqualToDict`.
Which I guess means numpy is doing some checks on q that is leading to this error? And perhaps if there is an alternate Drake function that works on Formulas this wouldn't happen?
I'm thinking a solution would be to use a port switch to implement the sign function, and saturation to implement saturation and compose blocks to feed into SymbolicVectorSystem. I was just wondering if there was something even simpler I was missing, and if this is the best way.
Context: I'm playing around with robust control and would like to implement dynamics and controllers that use the sign and saturation functions
We have min, max, and if_then_else, which should do the trick.
from pydrake.all import Variable, if_then_else, min, max
q = Variable('q')
#u = np.sign(q) # Doesn't work, but we can do
u = if_then_else(q > 0, 1, if_then_else(q < 0, -1, 0))
# u = np.clip(q, -1, 1) # Doesn't work, but we can do
u = min(max(q, -1), 1)
We actually also have clamp in c++, but somehow didn't add the python binding for that more recent addition. It would be easy enough to add.