-
Notifications
You must be signed in to change notification settings - Fork 0
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Input traj Update with navigation changes #1
Conversation
…because it's calling others methods with lock_guards)
… intermediate poses -> doesn't work on actual implementation
… motion that the robot has to take on each pair of poses in the path
…ed path. Added a sort of shim controller of the angle difference between two consecutive path poses is too big
…ructure. Changed the constraint function check
…tituiting the original first one. Started handling orientation angles periodicity -pi +pi
… both in manual or navigation mode. Exposed methods for handling config. WIP
…called for the computation (or interpolation) of the new steps. Added method for passing externally the navigation path for interpolation.
…ation loop with iterators for better handling (bugged before). Resetted global variable m_integratedPath -> TODO change it to local
…ous step. Added support for generating footsteps in place if no navigation path is given
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Thanks for the big work.
I left a few comments after skimming through the code that are mainly curiosities and trivial questions of mine.
@SimoneMic , looking at the logs of the CI tests, I found the following
I dropped a comment of a possible fix. |
I added
for the math constants in c++ |
double sin_theta = std::abs(sin(slope_angle)); | ||
|
||
//if both poses aligned to the path = 0, if both poses aligned but not with the path > 0, if both poses perpendicular to the path = 1 | ||
double proprotionFactor = (std::abs(slope_angle - navigationPath[i+1].angle) + std::abs(slope_angle - navigationPath[i].angle))/ 2 / M_PI_2 ; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
according to the MS documentation , to use the M_PI
, you need additional MS stuff, as in doing in the header also the line
_USE_MATH_DEFINES
before doing #include cmath
Thanks @SimoneMic. Since also the PR was tested against |
Opening this PR for the integration of a custom method for computing the trajectory from an omnidirectional unicycle following a navigation path.
Points of discussion:
enum struct NavigationSetup
has been created for allowing multiple uses of the library. This is by default inManualMode
(i.e. usual joystick command in X and Y).This is configure by the method
setPlannerMode(...)
. This setup switches the call to the method ofcomputeNewSteps()
orinterpolateNewSteps()
struct PoseStamped
has been created for tracking the poses computed in the trajectory.checkConstraints()
is being used ininterpolateNewSteps()
for checking the satisfaction of the constraints. It was taken by the tests code and improved.setInputPath()
and stored instd::vector<UnicycleState> m_inputPath
m_currentController
has been moved to public (since needs to be accessed by the walking-controller by the TrajectoryGenerator) -> could create a dedicated methodWe can discuss here what to improve and is yet to test this if it works with the actual
master
of thewalking-controller