Skip to content
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

Merged
merged 42 commits into from
Apr 4, 2023
Merged

Input traj Update with navigation changes #1

merged 42 commits into from
Apr 4, 2023

Conversation

SimoneMic
Copy link

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:

  • A new enum struct NavigationSetup has been created for allowing multiple uses of the library. This is by default in ManualMode (i.e. usual joystick command in X and Y).
    This is configure by the method setPlannerMode(...). This setup switches the call to the method of computeNewSteps() or interpolateNewSteps()
  • Also a new struct PoseStamped has been created for tracking the poses computed in the trajectory.
  • A new method checkConstraints() is being used in interpolateNewSteps() for checking the satisfaction of the constraints. It was taken by the tests code and improved.
  • The navigation path is being passed by the method setInputPath() and stored in std::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 method
  • Still present some debug/info output that could be removed

We can discuss here what to improve and is yet to test this if it works with the actual master of the walking-controller

Simone Micheletti and others added 30 commits October 18, 2022 17:29
…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
Copy link
Owner

@mebbaid mebbaid left a 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.

src/UnicycleGenerator.cpp Outdated Show resolved Hide resolved
src/UnicycleGenerator.cpp Show resolved Hide resolved
tests/UnicycleTest.cpp Show resolved Hide resolved
@mebbaid
Copy link
Owner

mebbaid commented Apr 4, 2023

@SimoneMic , looking at the logs of the CI tests, I found the following

2023-04-04T07:28:32.3224826Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1179,142): error C2065: 'M_PI_2': undeclared identifier [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3228938Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1190,31): error C2065: 'M_PI': undeclared identifier [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3232784Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1194,60): error C2065: 'M_PI': undeclared identifier [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3236354Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1198,60): error C2065: 'M_PI': undeclared identifier [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3239835Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1210,32): error C2065: 'M_PI': undeclared identifier [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3247158Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1212,61): error C2065: 'M_PI': undeclared identifier [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3259797Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1224,35): warning C4244: 'initializing': conversion from 'double' to 'int', possible loss of data [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3261283Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1235,39): error C2065: 'M_PI': undeclared identifier [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3263544Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1237,42): error C2065: 'M_PI': undeclared identifier [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3265245Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1239,45): error C2065: 'M_PI': undeclared identifier [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3267070Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1241,42): error C2065: 'M_PI': undeclared identifier [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3268089Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1334,27): error C2065: 'M_PI': undeclared identifier [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3273940Z D:\a\unicycle-footstep-planner\unicycle-footstep-planner\src\UnicyclePlanner.cpp(1336,51): error C2065: 'M_PI': undeclared identifier [D:\a\unicycle-footstep-planner\unicycle-footstep-planner\build\UnicyclePlanner.vcxproj]
2023-04-04T07:28:32.3788852Z   UnicycleBaseController.cpp

I dropped a comment of a possible fix.

@SimoneMic
Copy link
Author

I added

#define _USE_MATH_DEFINES
#include <cmath>

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 ;
Copy link
Owner

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

@mebbaid
Copy link
Owner

mebbaid commented Apr 4, 2023

I added

#define _USE_MATH_DEFINES
#include <cmath>

for the math constants in c++

Thanks @SimoneMic. Since also the PR was tested against ergoCubGazeboV1, I will merge.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
2 participants