Add trajopt_ifopt and trajopt_sqp #116

mpowelson · 2019-07-22T17:10:56Z

This PR will add costs and constraints using IFOPT as a generic solver interface. This will allow the usage of solvers such as IPOPT, SNOPT, Pagmo, and others.

Costs/Constraints

Currently Supported Constraints :

Joint Position
Joint Velocity
Cartesian Position(FK)
Inverse Kinematics

Currently Supported Costs (converts any constraint into a cost)

Squared Cost

Solvers

Additionally, the SQP solver has been rewritten. Currently it provides the SQP routine with interfaces to these QP solvers

OSQPEigen

Therefore the solvers that will work in this new framework are

IPOPT
SNOPT
Trajopt_sqp

Breaking Changes

IFOPT has been added to rosinstall. While it is released on the ROS build farm, this currently needs some changes that have not been released yet.

mpowelson · 2019-07-22T17:12:12Z

@Levi-Armstrong Could you take a quick look at this? I've got joint position variables and joint position constraint. Mostly I'd like to make sure I'm not doing something blatantly wrong, have the cmake set up correctly, have tests set up correctly, etc.

Adds the trajopt IFOPT package. Includes joint position and velocity constraints as well as the squared error cost. Includes 2 small examples of usage with IPOPT

mpowelson · 2020-04-21T23:02:34Z

@Levi-Armstrong I think this is probably all of the costs/cnts I will do for now. I'll transition to working on the solver interface now.

trajopt_ifopt/CMakeLists.txt

trajopt_ifopt/cmake/trajopt_ifopt-config.cmake.in

trajopt_ifopt/package.xml

trajopt_ifopt/src/cartesian_position_constraint.cpp

trajopt_ifopt/src/collision_constraint.cpp

trajopt_ifopt/src/joint_position_constraint.cpp

trajopt_ifopt/src/joint_velocity_constraint.cpp

If set, the state is pulled from the environment rather than from the frozen state solver fix clang

I added these methods to make them easier to call without an IFOPT problem. The joint terms didn't seem to make a lot of sense because they operate on multiple ifopt variables.

The old method solved a QP to get a feasible point, but that could sometimes change the starting point from the one passed in.

Adds a flag to the SQP optimizer and the OSQP interface. If both are true, then it prints all SQP Results, writes the model to a file in a human readable format, and prints the optimizer matrices to the terminal.

These are necessary since the callbacks are divided up now and not associated with the cost terms themselves. To replicate trajopt_sco behavior add a clear plotter callback, then the cost term callbacks, and finally the wait for input.

mpowelson · 2020-05-18T20:39:59Z

@Levi-Armstrong You don't have any problems with the new code in this PR being Apache 2 do you? I was going to add the notices.

Levi-Armstrong · 2020-05-18T21:03:16Z

Nope. That is the preferred license.

mpowelson · 2020-05-18T22:11:16Z

@Levi-Armstrong I think this is ready for you to review.

Disclaimers

I've got some TODOs in the trajopt_ifopt readme.
Also, the OSQPEigen solver interface still needs some work as well. I'm just calling the solver directly in TrustRegionSQP instead of using the base class. I've got a branch where I am working on that, but since this is functional, I think getting this in is probably more important.

Levi-Armstrong

I have reviewed half of the files. In addition to the provided comments it looks like the tests withing trajopt_ifopt is missing license information.

trajopt/CMakeLists.txt

dependencies.rosinstall

trajopt_ext/osqp_eigen/package.xml

trajopt_ifopt/cmake/trajopt_ifopt-config.cmake.in

trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h

trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h

trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h

trajopt_ifopt/test/CMakeLists.txt

trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/osqp_eigen_solver.h

trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_problem.h

trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/qp_solver.h

trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/trust_region_sqp_solver.h

trajopt_optimizers/trajopt_sqp/src/osqp_eigen_solver.cpp

trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp

trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp

trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp

mpowelson · 2020-05-26T23:13:21Z

@Levi-Armstrong This is ready for another look.

Levi-Armstrong

Minor changes.

trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h

trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h

trajopt_ifopt/test/cartesian_position_constraint_unit.cpp

trajopt_ifopt/test/cast_cost_unit.cpp

trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp

trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp

trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp

mpowelson · 2020-05-27T15:00:23Z

@Levi-Armstrong Addressed the new round of comments

mpowelson · 2020-05-27T15:01:13Z

Sorry. Forgot to run clang-format. Now it is ready.

This includes cleaning up the OSQPEigenSolver interface and a lot of style changes.

mpowelson · 2020-05-27T15:17:36Z

Squashed the last 3 commits now that you have looked at it.

Levi-Armstrong · 2020-05-27T17:32:50Z

I will merge once CI passes.

mpowelson · 2020-05-27T17:56:17Z

We will have to add ifopt to any downstream packages' rosinstalls.

mpowelson force-pushed the feature/ifopt branch 2 times, most recently from 4e3c781 to f9af683 Compare July 22, 2019 18:35

TrajOpt IFOPT: Joint Level Costs/Constraints

5b1b28d

Adds the trajopt IFOPT package. Includes joint position and velocity constraints as well as the squared error cost. Includes 2 small examples of usage with IPOPT

mpowelson force-pushed the feature/ifopt branch from f9af683 to 5b1b28d Compare April 17, 2020 21:43

mpowelson added 4 commits April 20, 2020 14:03

Install trajopt test data for use in other packages

c67719b

Add Cartesian Position Constraint

fa216df

Clang Tidy Cleanup

2f39da2

Add IFOPT Collision Constraint

2647ac5

Levi-Armstrong reviewed Apr 23, 2020

View reviewed changes

trajopt_ifopt/CMakeLists.txt Outdated Show resolved Hide resolved

Levi-Armstrong requested changes Apr 23, 2020

View reviewed changes

mpowelson added 2 commits April 24, 2020 16:40

Add OSQP_Eigen to Trajopt_Ext

429374e

Cleanup based on review comments

e97fc91

mpowelson force-pushed the feature/ifopt branch from b2441fd to 165cf5a Compare April 29, 2020 19:20

Add SQP solver based on IFOPT

051694b

mpowelson force-pushed the feature/ifopt branch from 165cf5a to 051694b Compare April 30, 2020 19:36

mpowelson added 5 commits May 1, 2020 16:03

trajopt_ifopt bug fixes

dabe4c6

Add utilities and convenience functions for setting JointPosition bounds

43841f6

Trajopt_ifopt: Minor Enhancements

094514e

Add Flag to collision evaluator for dynamic environments

541069d

If set, the state is pulled from the environment rather than from the frozen state solver fix clang

Trajopt_ifopt: Add InverseKinematicsConstraint and CartPos Unit Test

80bdcd7

mpowelson force-pushed the feature/ifopt branch from f2c258f to 80bdcd7 Compare May 6, 2020 15:22

mpowelson added 4 commits May 6, 2020 18:05

Trajopt_ifopt: Add utility to get closest valid point within bounds

9a22103

Trajopt_ifopt: Add CalcValue and CalcJacobian methods to constraints

62cca5d

I added these methods to make them easier to call without an IFOPT problem. The joint terms didn't seem to make a lot of sense because they operate on multiple ifopt variables.

Add numeric jacobian calculation to cartesian cost unit tests

89b5384

Add getClosestFeasiblePoint that doesn't use a QP

68b13f4

The old method solved a QP to get a feasible point, but that could sometimes change the starting point from the one passed in.

mpowelson force-pushed the feature/ifopt branch from 6b3fd0e to f777d12 Compare May 8, 2020 15:25

Trajopt_ifopt: Simple Readme

cfb2b03

mpowelson force-pushed the feature/ifopt branch from f777d12 to cfb2b03 Compare May 8, 2020 15:26

Add super debug mode to trajopt_sco

519704a

Adds a flag to the SQP optimizer and the OSQP interface. If both are true, then it prints all SQP Results, writes the model to a file in a human readable format, and prints the optimizer matrices to the terminal.

mpowelson force-pushed the feature/ifopt branch from 81f49c6 to 519704a Compare May 15, 2020 14:04

mpowelson added 3 commits May 15, 2020 15:07

Improve trajopt_sqp debug printouts

a2fbdc9

trajopt_sqp: Convert examples into unit tests

5af6c81

mpowelson added 2 commits May 18, 2020 16:14

trajopt_ifopt/trajopt_sqp: Add Apache 2 license notices

f77ba85

trajopt_ifopt: Misc cleanup for pull request

efc4a52

mpowelson force-pushed the feature/ifopt branch from a8cee7a to efc4a52 Compare May 18, 2020 22:00

mpowelson changed the title ~~WIP: Feature/ifopt~~ Feature/ifopt May 18, 2020

mpowelson changed the title ~~Feature/ifopt~~ Add trajopt_ifopt and trajopt_sqp May 18, 2020

Levi-Armstrong reviewed May 21, 2020

View reviewed changes

mpowelson force-pushed the feature/ifopt branch from bf7f0df to 5bc070b Compare May 26, 2020 23:12

Levi-Armstrong requested changes May 27, 2020

View reviewed changes

mpowelson force-pushed the feature/ifopt branch from f0c281f to add6d3d Compare May 27, 2020 15:00

trajopt_ifopt/trajopt_sqp: Changes after review

9a13918

This includes cleaning up the OSQPEigenSolver interface and a lot of style changes.

mpowelson force-pushed the feature/ifopt branch from add6d3d to 9a13918 Compare May 27, 2020 15:16

Fix trajopt_sqp cart_position_optimization_unit test

cf94d4d

Levi-Armstrong approved these changes May 27, 2020

View reviewed changes

Levi-Armstrong merged commit 85e8d41 into tesseract-robotics:master May 27, 2020

mpowelson deleted the feature/ifopt branch May 27, 2020 17:53

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add trajopt_ifopt and trajopt_sqp #116

Add trajopt_ifopt and trajopt_sqp #116

mpowelson commented Jul 22, 2019 •

edited

Loading

mpowelson commented Jul 22, 2019

mpowelson commented Apr 21, 2020

mpowelson commented May 18, 2020

Levi-Armstrong commented May 18, 2020

mpowelson commented May 18, 2020

Levi-Armstrong left a comment

mpowelson commented May 26, 2020

Levi-Armstrong left a comment

mpowelson commented May 27, 2020

mpowelson commented May 27, 2020

mpowelson commented May 27, 2020

Levi-Armstrong commented May 27, 2020

mpowelson commented May 27, 2020

Add trajopt_ifopt and trajopt_sqp #116

Add trajopt_ifopt and trajopt_sqp #116

Conversation

mpowelson commented Jul 22, 2019 • edited Loading

Costs/Constraints

Solvers

Breaking Changes

mpowelson commented Jul 22, 2019

mpowelson commented Apr 21, 2020

mpowelson commented May 18, 2020

Levi-Armstrong commented May 18, 2020

mpowelson commented May 18, 2020

Levi-Armstrong left a comment

Choose a reason for hiding this comment

mpowelson commented May 26, 2020

Levi-Armstrong left a comment

Choose a reason for hiding this comment

mpowelson commented May 27, 2020

mpowelson commented May 27, 2020

mpowelson commented May 27, 2020

Levi-Armstrong commented May 27, 2020

mpowelson commented May 27, 2020

mpowelson commented Jul 22, 2019 •

edited

Loading