search

robomechanics / quad-sdk

Software tools for agile quadrupeds, developed by the Robomechanics Lab at Carnegie Mellon University.
https://robomechanics.github.io/quad-sdk/
MIT License
713 stars 133 forks source link

Improve local planner solve when yaw wraps #204

Closed jcnorby closed 10 months ago

jcnorby commented 2 years ago

Right now if a twist input is commanded to spin in a circle, we get a much slower solve when the robot crosses from yaw = -pi to yaw = pi. Short term solution would be to update the warm start to unwrap yaw if it crosses that threshold. Longer term solution would be to use quaternions or some other representation.

yanhaoy commented 2 years ago

Will try to wrap yaw in warmstart now, not sure will it affect the multipliers. Using a quaternion in direct collocation requires an extra slack variable to keep it as a unit vector.

dhajnes commented 1 year ago

Hi, any news on this from the dev's side? It's a year and a half old problem that has allegedly "a pretty easy solution, we just haven't gotten around to it yet" citing #345. @jcnorby

ardalantj commented 1 year ago

Hi @dhajnes, Our team is currently working on other higher priority like state estimation. We may or may not get to this in the near future.

jcnorby commented 1 year ago

@dhajnes sorry it took so long to get to this! Finally found a time and opened a PR (#400) that resolves this issue. Feel free to merge into your branch, although its pretty self contained so should be merged with devel pretty soon.

Thanks for your interest!