The Drake developers at TRI have an implementation of this algorithm in our internal repositories. In principle, we are happy to share it, but making it public takes time and we have not yet prioritized it. Please upvote it by adding a "thumbs up" reaction to this comment if you want us to know that you are keen.
The Drake developers at TRI have an implementation of this algorithm in our internal repositories. In principle, we are happy to share it, but making it public takes time and we have not yet prioritized it. Please upvote it by adding a "thumbs up" reaction to this comment if you want us to know that you are keen.
Original feature request from @tarquasso .