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.

Sign up for GitHub

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

Jump to bottom

Fixed horizon and other bugs in examples #100

Merged
merged 9 commits into from
Apr 2, 2021
Merged

Fixed horizon and other bugs in examples #100

merged 9 commits into from
Apr 2, 2021

Conversation

scharalambous3
Copy link
Collaborator

Fixed #94

@scharalambous3
Copy link
Collaborator Author

scharalambous3 commented Mar 29, 2021 •
edited
Loading

Fixed DynamicsGraph call to reflect update in constructor following discussion w/ @varunagrawal

@scharalambous3
Copy link
Collaborator Author

I will also fix an error due to TorqueKey, JointAngleKey and JointVelKey that arises in two of the examples

@scharalambous3 scharalambous3 changed the title Fixed horizon bug in Hermite parametrization Fixed horizon and other bugs in examples Mar 29, 2021
@scharalambous3 scharalambous3 added bugfix Fixes an issue or bug documentation Improvements or additions to documentation labels Mar 29, 2021
@scharalambous3 scharalambous3 self-assigned this Mar 29, 2021
@dellaert
Copy link
Member

@varunagrawal please review this before we merge #111 as some conflict...
@scharalambous3 you should request reviews :-) Is this ready to go in your opinion?

@scharalambous3
Copy link
Collaborator Author

scharalambous3 commented Mar 30, 2021 •
edited
Loading

@dellaert Sorry for that, will do from now on! Yes I think so - these changes resolved the errors I was getting in building and executing the examples

varunagrawal
varunagrawal requested changes Mar 30, 2021
View reviewed changes
Copy link
Collaborator

@varunagrawal varunagrawal left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

We need references so we can cross verify the math.

examples/example_quadruped_mp/main.cpp Outdated Show resolved Hide resolved
examples/example_quadruped_mp/main.cpp Outdated Show resolved Hide resolved
examples/example_quadruped_mp/main.cpp Outdated Show resolved Hide resolved
examples/example_quadruped_mp/main.cpp Outdated Show resolved Hide resolved
@varunagrawal
Copy link
Collaborator

varunagrawal commented Mar 30, 2021 •
edited
Loading

Ah I get it now. uE[0,1] should instead be u ∈ [0,1] (aka unicode magic). Thanks @scharalambous3.

Also, do you want me to handle the matrix multiplication? I have an idea to really simplify this code.

@scharalambous3
Copy link
Collaborator Author

Yes that will be great!

@varunagrawal
Copy link
Collaborator

I also think there's a bug in the CsvWriter since my traj.csv is empty. Does it work for you @scharalambous3?

@scharalambous3
Copy link
Collaborator Author

Yes that's right, mine is empty too

@varunagrawal
Copy link
Collaborator

Looks like @dellaert introduced the bug in line 232. There is an extra pose_file.open() which should not be there. Didn't unit test, tch-tch.

@varunagrawal
Copy link
Collaborator

@scharalambous3 I just pushed up my update. Check out how clean the new code is (plus lines up perfectly with the math)!

I verified the result by running a diff on the two traj.csv files, one generated by the old code and one generated by mine.

dellaert
dellaert requested changes Mar 30, 2021
View reviewed changes
Copy link
Member

@dellaert dellaert left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Great opportunity to get rid of copy/pasta, maybe make a new function in initialization utils. But make sure it does not already exist...

examples/example_cart_pole_trajectory_optimization/main.cpp
graph.addPrior(JointVelKey(j0_id, 0), X_i[1], dynamics_model);
graph.addPrior(JointAngleKey(j1_id, 0), X_i[3], dynamics_model);
graph.addPrior(JointVelKey(j1_id, 0), X_i[4], dynamics_model);
graph.addPrior(internal::JointAngleKey(j0_id, 0), X_i[0], dynamics_model);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy/pasta
I think there must be a function already to do this, and if not, there should be

examples/example_inverted_pendulum_trajectory_optimization/main.cpp

// Add initial conditions to trajectory factor graph.
graph.addPrior(JointAngleKey(j1_id, 0), theta_i, dynamics_model);
graph.addPrior(JointVelKey(j1_id, 0), dtheta_i, dynamics_model);
graph.addPrior(internal::JointAngleKey(j1_id, 0), theta_i, dynamics_model);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The same copy/pasta!!!!!!

examples/example_quadruped_mp/main.cpp Outdated Show resolved Hide resolved
varunagrawal
varunagrawal approved these changes Mar 30, 2021
View reviewed changes
Copy link
Collaborator

@varunagrawal varunagrawal left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I am approving this since the code seems satisfactory now. Thanks for the PR @scharalambous3!!

@varunagrawal
Copy link
Collaborator

@scharalambous3 will leave you to handle @dellaert's comments. 🙂

@scharalambous3
Copy link
Collaborator Author

I could not find a function that serves that purpose of associating the desired link and state term
@varunagrawal I will get on it tomorrow due to an interview! If #111 is urgent though we can merge and I'd submit a new PR

@varunagrawal
Copy link
Collaborator

Nope not urgent. You should create the function, it'll be super useful! Good luck for the interview 🙂

Alescontrela
Alescontrela approved these changes Apr 1, 2021
View reviewed changes
Copy link
Member

@Alescontrela Alescontrela left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Note that I had to build this branch without the Python wrapper otherwise I received the following error:

/Applications/Xcode.app/Contents/Developer/Toolchains/XcodeDefault.xctoolchain/usr/bin/../include/c++/v1/memory:1841:16: error:
      'pointer' declared as a pointer to a reference of type 'const Eigen::Matrix<double, -1, 1, 0, -1, 1> &'
    typedef _Tp*              pointer;

(even though the wrapper builds in master). @varunagrawal Any ideas?

Other than that, all the examples build and produce nice trajectories :)

Cart pole trajectory optimization:

trajopt

Quadruped Static Motion Planning:

image

examples/example_quadruped_mp/main.cpp
coeffs.push_back(a_3);

return coeffs;
gtsam::Matrix43 C;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is much much cleaner, thanks!

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You can thank Sehoon Ha for teaching me all about splines last Fall. 🙂

@varunagrawal
Copy link
Collaborator

@Alescontrela I can't reproduce the issue. Then again I put in a lot of new fixes to wrap to address a bunch of things so maybe try pulling the latest version?

@dellaert dellaert merged commit 1d601da into master Apr 2, 2021
@dellaert dellaert deleted the fix/quadruped branch April 2, 2021 19:47
@scharalambous3 scharalambous3 mentioned this pull request Apr 5, 2021
Closed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@varunagrawal varunagrawal varunagrawal approved these changes

@Alescontrela Alescontrela Alescontrela approved these changes

@dellaert dellaert Awaiting requested review from dellaert

Assignees

@scharalambous3 scharalambous3

Labels
bugfix Fixes an issue or bug documentation Improvements or additions to documentation
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.

Quadruped example
4 participants
@scharalambous3 @dellaert @varunagrawal @Alescontrela