Closed yangwill closed 2 years ago
The current code looks like an anachronism, from before GetPositionsAndVelocities
existed (or I before I knew of it). It seems very reasonable to make the changes your requested.
However, I'm not sure why the current code doesn't work. The code here is very similar, without some bells and whistles, to what Drake does under the hood for GetPositionsAndVelocities
From what I can tell, some reasons that it might fail, though there might be others, are:
When trying to use HybridDircon on the Cassie model with springs, line 50 in
dircon_position_data.cc
and line 35 indircon_distance_data.cc
were causing issues. I'm not sure why it was working for the models without springs, but I think the line should beconst auto x = this->plant_.GetPositionsAndVelocities(context)
to retrieve the actual state.