Added DCMTrajectory generator and FeetMinimumJerkGenerator #17

S-Dafarra · 2018-12-17T08:01:39Z

This PR merges the branch dcmTrajectoryGenerator even tough not in its current form. Indeed, a general refactory was necessary in order to allow merging in devel. Thus new bugs may arose 😅

For the time being I just run the tests that were added to that branch, but I would kindly ask @GiulioRomualdi to perform more thorough tests.

The code has been copied from the dcmTrajectoryGenerator branch and refactored to comply with the new structure of the planner.

GiulioRomualdi · 2018-12-17T10:19:27Z

src/UnicycleGenerator.cpp

+    std::lock_guard<std::mutex> guard(m_pimpl->mutex);
+
+    if (m_pimpl->feetMinimumJerkGenerator == nullptr) {
+        m_pimpl->feetMinimumJerkGenerator.reset(new FeetMinimumJerkGenerator());


Instead of using

m_pimpl->feetMinimumJerkGenerator.reset(new FeetMinimumJerkGenerator());

You may use

m_pimpl->feetMinimumJerkGenerator = std::make_shared<FeetMinimumJerkGenerator>();

Here and here you can find some reasons why this is in general better. In simple terms it just a matter of heap-allocation. Using std::make_shared only one heap-allocation is performed while, on the other hand, new FeetMinimumJerkGenerator() invokes a heap-allocation for the managed data and the .reset( ) method performs another one.

Unfortunately it is not possible. The constructors are private and can be allocated only because UnicyleGenerator is a friend. As a consequence std::make_shared fails is allocating those objects, while with new we circumvent this issue.

Feet twists are also an output of the cubic spline generator. Some bugfix: - the linear velocity in the minimum jerk generator was not set - ignoring other QTCreator files

S-Dafarra · 2019-01-18T12:38:24Z

Together with robotology/walking-controllers#23, I tested this PR in simulation and on the real robot. It seems to be working fine. @GiulioRomualdi if you could please take a look at the code if it is reasonable and/or perform other tests. Then we could merge.

GiulioRomualdi

Only some minor comments

GiulioRomualdi · 2019-01-18T18:46:52Z

CMakeLists.txt

@@ -28,7 +28,7 @@ include(CMakePackageConfigHelpers)

 project(UnicyclePlanner
        LANGUAGES CXX
-        VERSION 0.1.100)
+        VERSION 0.1.200)


Just a curiosity, how are we handing the versioning of the code?

In what are you interested in? The numbering system or how this is used inside CMake?

Actually it is better 102 for the patch version.

include/DCMTrajectoryGenerator.h

GiulioRomualdi · 2019-01-18T19:03:51Z

src/UnicycleGenerator.cpp

@@ -157,10 +170,10 @@ class UnicycleGenerator::UnicycleGeneratorImplementation {
                //bool pause = m_pauseActive && (switchTime > m_maxSwitchTime); //if true, it will pause in the middle
                size_t mergePoint;
                if (pause){
-                    mergePoint = phaseShift.back() - static_cast<size_t>(std::round(nominalSwitchTime/(2*dT)));
+                    mergePoint = phaseShift.back() - static_cast<size_t>(std::round(nominalSwitchTime * (1 - mergePointRatio)/(dT)));


Why did you change the way to evaluate the new mergePoint during the pause condition?

You tell me 😉 https://github.com/robotology/unicycle-footstep-planner/blame/98f649aaa037bfbabeb95735a8a9364910e87903/src/FootPrintsInterpolator.cpp#L164

GiulioRomualdi · 2019-01-18T19:31:55Z

src/FeetMinimumJerkGenerator.cpp

+                    }
+                    output[instant] = newTransform;
+                    iDynTree::toEigen(rightTrivializedAngVelocity) = iDynTree::toEigen(iDynTree::Rotation::RPYRightTrivializedDerivative(0.0, pitchAngle, yawAngle)) *
+                            iDynTree::toEigen(rpyDerivative);


I've found an elegant way to evaluate the minimum jerk trajectory in SE(3). Here (page 162) you can find further details. Using this approach we can avoid to evaluate the minimum jerk trajectory for RPY and then compose the rotation matrix. Also because I'm not sure that the two approaches (minJerk(RPY) -> rotationMatrix and directly (minJerk(rotationMatrix)) return the same results. @DanielePucci

By the way, for the time being the approach that we are using is fine.

Feel free to open a PR for this 😉

Here (page 162) you can find further details.

I met that guy at a conference. By far the best walking I ever saw on a small humanoid (in his case it was the Nao), really impressive.

You can find his code at https://github.com/mrsp/serow .

GiulioRomualdi · 2019-01-18T19:33:59Z

src/FeetMinimumJerkGenerator.cpp

+
+
+    bool interpolateFoot(double dT, const std::vector<size_t>& phaseShift, const std::vector<StepPhase> &stepPhase,
+                         const FootPrint &foot, std::vector<iDynTree::Transform> &output, std::vector<iDynTree::Twist> &outputTwistsInMixedRepresentation) {


It would be nice to have also the acceleration of the feet, indeed it may be useful for the torque control. I know that it's not easy to retrieve but we should discuss some possible solutions.

What is missing is the relation between the RPY double derivative and the angular acceleration. From the outside it is not clear that inside the RPY parametrization is used, thus I would avoid to output the RPY double derivative instead of the actual acceleration. We may address this in a separate PR.

src/FeetMinimumJerkGenerator.cpp

GiulioRomualdi · 2019-01-18T22:34:07Z

src/UnicycleGenerator.cpp

        }
        return true;
    }

-    bool createPhasesTimings(const FootPrint &leftFootPrint, const FootPrint &rightFootPrint) {
+    bool createPhasesTimings(const FootPrint &lFootPrint, const FootPrint &rFootPrint) {
        //NOTE this method must be called after orderSteps to work properly

        lFootPhases.reset(new std::vector<StepPhase>());


I know that this part was already here, however, I want to ask you two things:

lFootPhases is an attribute of the class. Why is it not like m_lFootPhases? This is general for all the attribute of UnicycleGenerator::UnicycleGeneratorImplementation class. Is this related to the fact that all these attributes are public?

probabily here you can use std::make_shared<>() instead of .reset(new )

Regarding 1. I usually use the m_* convention only for private members. Since it is an implementation class, all the parameters are public, so that they can be accessed as m_pimpl->parameter instead of m_pimpl->m_parameter. A grey area is when you have methods in the implementation class which use the public parameters. I agree that readability degrades a bit, but I prefer to stick to the initial decision.

Regarding 2., you are right I am going to change it.

Done 2. in 5332a08.

GiulioRomualdi · 2019-01-18T23:01:02Z

src/UnicycleGenerator.cpp

@@ -641,4 +694,15 @@ std::shared_ptr<CoMHeightTrajectoryGenerator> UnicycleGenerator::addCoMHeightTra
    return m_pimpl->comHeightGenerator;
 }

+std::shared_ptr<DCMTrajectoryGenerator> UnicycleGenerator::addDCMTrajectoryGenerator()


As far as I know one returns a shared_ptr if one wants to have the ownership attached to the pointer, but I think that here the ownership should belong only to the UnicycleGenerator object. So I suggest to make m_pimpl->dcmTrajectoryGenerator an unique_ptr and return the raw pointer m_pimpl->dcmTrajectoryGenerator.get()
What do you think?

I prefer not to return the raw pointer, otherwise the user class will have to check the existence of that generator every time it tries to access it. In the way you propose, all the generators would be deleted as soon as the UnicycleGenerator will go out of scope. Instead, by using a shared pointer, the generators will still be accessible (thus you can still access the computed trajectories if needed) without the need to check the pointer, but it will not be possible to generate new trajectories.

S-Dafarra · 2019-01-21T16:16:20Z

@GiulioRomualdi when you are happy with the comments we can merge 😉

GiulioRomualdi · 2019-01-23T09:23:56Z

LGTM. You can merge the PR

S-Dafarra added 2 commits December 13, 2018 12:18

Added DCMTrajectoryGenerator and FeetMinimumJerkGenerator classes.

6252727

The code has been copied from the dcmTrajectoryGenerator branch and refactored to comply with the new structure of the planner.

Added tests for dcm generation and bug fixing.

731dd87

S-Dafarra self-assigned this Dec 17, 2018

S-Dafarra requested a review from GiulioRomualdi December 17, 2018 08:01

Ported also the possibility to set the mergePointRatio.

8188a33

S-Dafarra mentioned this pull request Dec 17, 2018

Modifications to use the new version of the unicycle planner. robotology/walking-controllers#23

Merged

GiulioRomualdi reviewed Dec 17, 2018

View reviewed changes

The feet generators now inherit from a virtual base class.

67ff2e7

Feet twists are also an output of the cubic spline generator. Some bugfix: - the linear velocity in the minimum jerk generator was not set - ignoring other QTCreator files

S-Dafarra mentioned this pull request Dec 19, 2018

Added files to add osqp, OsqpEigen, UnicyclePlanner and walking-controllers robotology/robotology-superbuild#87

Merged

8 tasks

S-Dafarra added 3 commits January 17, 2019 16:13

Fixed bug when the unicycle does not move.

cd8eb98

Bugfix in MinimumJerkGenerator.

01a5167

Fixed bug when computing last step.

dc4284d

GiulioRomualdi reviewed Jan 18, 2019

View reviewed changes

Cleanup.

5332a08

Changed patch version to match master/devel versioning.

0fca5d8

S-Dafarra merged commit ef71c9a into devel Jan 23, 2019

This was referenced Jan 24, 2019

Merge the dcmTrajectoryGenerator branch #14

Closed

Implement a getter for the list of ordered steps #5

Closed

S-Dafarra deleted the dcm-refactoring branch October 27, 2020 14:19

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Added DCMTrajectory generator and FeetMinimumJerkGenerator #17

Added DCMTrajectory generator and FeetMinimumJerkGenerator #17

S-Dafarra commented Dec 17, 2018

GiulioRomualdi Dec 17, 2018

S-Dafarra Dec 17, 2018

S-Dafarra commented Jan 18, 2019

GiulioRomualdi left a comment

GiulioRomualdi Jan 18, 2019

S-Dafarra Jan 21, 2019

S-Dafarra Jan 23, 2019

GiulioRomualdi Jan 18, 2019

S-Dafarra Jan 21, 2019

GiulioRomualdi Jan 18, 2019

S-Dafarra Jan 21, 2019

traversaro Jan 21, 2019

traversaro Jan 21, 2019

GiulioRomualdi Jan 18, 2019

S-Dafarra Jan 21, 2019

GiulioRomualdi Jan 18, 2019

S-Dafarra Jan 21, 2019

S-Dafarra Jan 21, 2019

GiulioRomualdi Jan 18, 2019

S-Dafarra Jan 21, 2019 •

edited

Loading

S-Dafarra commented Jan 21, 2019

GiulioRomualdi commented Jan 23, 2019



		bool interpolateFoot(double dT, const std::vector<size_t>& phaseShift, const std::vector<StepPhase> &stepPhase,
		const FootPrint &foot, std::vector<iDynTree::Transform> &output, std::vector<iDynTree::Twist> &outputTwistsInMixedRepresentation) {

Added DCMTrajectory generator and FeetMinimumJerkGenerator #17

Added DCMTrajectory generator and FeetMinimumJerkGenerator #17

Conversation

S-Dafarra commented Dec 17, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

S-Dafarra commented Jan 18, 2019

GiulioRomualdi left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

S-Dafarra Jan 21, 2019 • edited Loading

Choose a reason for hiding this comment

S-Dafarra commented Jan 21, 2019

GiulioRomualdi commented Jan 23, 2019

S-Dafarra Jan 21, 2019 •

edited

Loading