Add benchmark example for dircon #107

yminchen · 2019-10-21T19:56:51Z

Fix bugs in dircon and add a benchmark example for dircon's speed.

Detail changes:
Dircon:

fix a bug. Use the jacobian that doesn't skip any constraints in the equations of motion
fix bugs in the index of decision variable when adding constraint
pass in surface normal into constructor of dircon_position_data
add surface (e.g. ground) incline feature to dircon_position_data
use linear constraint instead of LorentzConeConstraint for friction cone (to increase speed)
add a utility function to detect if a MBP is quaternion floating based
add unit norm quaternion constraint and also add a slack variable in dynamic constraint

Example:

Trajectory optimization of Cassie squating

… Scale option=2 and taks 15 mins if Scale option=0); add ground incline for contact constraint; clean up code

…; need to use fix stance foot position to get better initial guess

…option = 0) in 2 mins

mposa · 2019-10-22T23:57:05Z

systems/trajectory_optimization/dircon_kinematic_data_set.cc

@@ -109,7 +110,7 @@ void DirconKinematicDataSet<T>::updateData(const Context<T>& context,
    right_hand_side_ = -right_hand_side_ +
        plant_.MakeActuationMatrix() * input +
        plant_.CalcGravityGeneralizedForces(context) +
-        getJ().transpose() * forces;
+        J_.transpose() * forces;


Should this be getJWithoutSkipping? That might be the best way to avoid a similar error down the road.

Yes, getJWithoutSkipping() is probably better here

mposa · 2019-10-22T23:58:19Z

systems/trajectory_optimization/dircon_opt_constraints.cc

@@ -26,6 +26,8 @@ using drake::VectorX;
 using drake::MatrixX;
 using Eigen::VectorXd;
 using Eigen::MatrixXd;
+using std::cout;


Probably better to not bloat files with using's for debugging code that shouldn't be merged to master anyway.

mposa · 2019-10-23T00:00:05Z

systems/trajectory_optimization/dircon_opt_constraints.h

@@ -69,15 +70,17 @@ class DirconDynamicConstraint : public DirconAbstractConstraint<T> {
  DirconDynamicConstraint(const drake::multibody::MultibodyPlant<T>& plant,
                          DirconKinematicDataSet<T>& constraints,
                          int num_positions, int num_velocities, int num_inputs,
-                          int num_kinematic_constraints);
+                          int num_kinematic_constraints_wo_skipping,
+                          int num_quat_slack);


What is num_quat_slack for? I know this file needs documentation badly, and that's on me, but it would be good to document any new additions.

num_quat_slack is the dimension of the slack variable for quaternion. It's always 1 if the MBP is quaternion floating based. Otherwise, it's 0. I'll add documentation about this.

Shouldn't this be documented in the header (here)?

systems/trajectory_optimization/dircon_opt_constraints.cc

mposa · 2019-10-23T00:38:39Z

systems/trajectory_optimization/dircon_position_data.h

@@ -13,7 +13,8 @@ class DirconPositionData : public DirconKinematicData<T> {
 public:
  DirconPositionData(const drake::multibody::MultibodyPlant<T>& plant,
    const drake::multibody::Body<T>& body,
-    Eigen::Vector3d pt, bool isXZ = false);
+    Eigen::Vector3d pt, bool isXZ = false,
+    Eigen::Vector2d surface_slope_roll_pitch = Eigen::Vector2d::Zero());


Wouldn't surface normal, as a Vector3d be easier? Roll-pitch is a little ambiguous.

I agree. I replaced roll and pitch with normal.

mposa · 2019-10-23T01:30:05Z

examples/Cassie/run_dircon_squatting.cc

+/// With the default initial guess, the solving time is about 100 seconds.
+
+// Constraint to fix the position of a point on a body (for initial guess)
+class BodyPonitPositionConstraint : public DirconAbstractConstraint<double> {


Typo in class name BodyPonitPositionConstraint

mposa · 2019-10-23T02:07:41Z

examples/Cassie/run_dircon_squatting.cc

+  ContactInfo contact_info(xa, idxa);
+
+  VectorXd q_desired = VectorXd::Zero(n_q);
+  q_desired << -0.0872062,


Where did these magic numbers come from (comment would be useful). Can these be rounded?

It came from inverse kinematics where cassie was standing on the ground.
I also added documentation and rounded some of the numbers.

examples/Cassie/run_dircon_squatting.cc

mposa · 2019-10-23T02:10:33Z

systems/trajectory_optimization/hybrid_dircon.cc


    // For N-1 timesteps, add a constraint which depends on the knot
    // value along with the state and input vectors at that knot and the
    // next.

    //Adding dynamic constraints
+    auto constraint = std::make_shared<DirconDynamicConstraint<T>>(plant_, *constraints_[i], num_quat_slack);


Shouldn't the quaternion constraint be created and added here, rather than in the main file?

Yes. I made this change in the new commits

…ed on pull request feedback (haven't add normal vector of contact surface yet)

…tion

yminchen · 2019-10-26T20:46:34Z

@mposa I updated the code and it's ready for another review

…sing

mposa

Some comments. It looks like you auto-formatted some of the white space, and a lot of it ended up looking a little off. Are you sure it's correct?

mposa · 2019-11-04T01:56:03Z

systems/trajectory_optimization/dircon_opt_constraints.cc

+template <typename T>
+QuaternionNormConstraint<T>::QuaternionNormConstraint() :
+    DirconAbstractConstraint<T>(1, 4,
+                                       VectorXd::Zero(1),


Spacing here looks off?

Yes, I forgot to update the code spacing of this file

mposa · 2019-11-04T01:59:02Z

systems/trajectory_optimization/dircon_opt_constraints.h

@@ -69,15 +70,17 @@ class DirconDynamicConstraint : public DirconAbstractConstraint<T> {
  DirconDynamicConstraint(const drake::multibody::MultibodyPlant<T>& plant,
                          DirconKinematicDataSet<T>& constraints,
                          int num_positions, int num_velocities, int num_inputs,
-                          int num_kinematic_constraints);
+                          int num_kinematic_constraints_wo_skipping,
+                          int num_quat_slack);


Shouldn't this be documented in the header (here)?

mposa · 2019-11-04T01:59:48Z

systems/trajectory_optimization/dircon_position_data.cc

-    isXZ_(isXZ) {
+    const Body<T>& body, Vector3d pt, bool isXZ,
+    Vector3d surface_normal) :
+        DirconKinematicData<T>(plant, isXZ ? 2 : 3),


Spacing here looks strange?

Yes, I'll update the code style of this file

mposa · 2019-11-04T02:06:29Z

systems/trajectory_optimization/dircon_position_data.cc

  TXZ_ << 1, 0, 0,
          0, 0, 1;
+
+  Vector3d z_hat(0,0,1);
+  surface_normal.normalize();


Does it make sense to supply a surface_normal value if isXZ = true? If these two arguments cannot be supplied together, then this should either be prevented via the API or, at minimum, checked at runtime.

Yes, I use both of them in the example of planar five-link robot example.

mposa · 2019-11-04T02:08:08Z

systems/trajectory_optimization/dircon_position_data.cc

-    normal_xz << normal(0)/L, normal(2)/L;
-    d_xz << -normal_xz(1), normal_xz(0);
+    // builds the basis
+    Vector2d normal_xz(0,1);


Related to earlier comment re: normal and isXZ. This looks like a change where, previously, we supported non-flat normal vectors for planar contact. Is this no longer the case? If not, why are we removing this functionality?

Currently there are c, cdot and cddot in dircon_kinemaitcs_data. We want c=0 in the normal direction of the surface, so we rotate the contact position into the surface frame. (I guess we can generalize this more by adding a translation, if there is a specific position where we want the robot to touch the surface? Currently the surface has to contain the global origin.)

Since we rotate c, cdot and cddot because of the above reason, we don't need to rotate it again in the addFixedNormalFrictionConstraints().

mposa · 2019-11-04T02:13:28Z

systems/trajectory_optimization/hybrid_dircon.cc

+                              vector<double> maximum_timestep,
+                              vector<DirconKinematicDataSet<T>*> constraints,
+                              vector<DirconOptions> options)
+    : MultipleShooting(plant.num_actuators(),


Spacing looks off here.

I couldn't tell how it is off. I used a code style formatter to format hybrid_dircon.cc.
The style should be the same (if I didn't miss any thing) as Drake's. https://github.com/RobotLocomotion/drake/blob/master/systems/trajectory_optimization/multiple_shooting.cc#L63-L92

mposa · 2019-11-04T02:13:35Z

systems/trajectory_optimization/hybrid_dircon.cc

+      num_modes_(num_time_samples.size()),
+      mode_lengths_(num_time_samples),
+      v_post_impact_vars_(NewContinuousVariables(
+          plant.num_velocities() *


Same comment. I couldn't tell how it is off. I think this is the same as drake's style?

mposa · 2019-11-04T02:14:40Z

systems/trajectory_optimization/hybrid_dircon.cc

+        constraints_[i]->countConstraintsWithoutSkipping()
+            * (num_time_samples[i] - 1), "v_c[" + std::to_string(i) + "]"));
+    // slack variables used to scale quaternion norm to 1 in the dynamic constraints.
+    quaternion_slack_vars_.push_back(NewContinuousVariables(


This isn't very readable. Why not

if (is_quaternion) { // Add variables }

I feel they are equally readable, but I will change to

if (is_quaternion) { // Add variables }

mposa · 2019-11-04T02:16:10Z

systems/trajectory_optimization/hybrid_dircon.cc


    // For N-1 timesteps, add a constraint which depends on the knot
    // value along with the state and input vectors at that knot and the
    // next.

+
+    //Adding quaternion norm constraint
+    if (multibody::isQuaternion(plant)) {


Should use the boolean you've created, is_quaternion here.

mposa · 2019-11-04T02:17:16Z

systems/trajectory_optimization/hybrid_dircon.cc

+    if (multibody::isQuaternion(plant)) {
+      auto quat_norm_constraint =
+          std::make_shared<QuaternionNormConstraint<T>>();
+      for (int j = (i == 0) ? 0 : 1; j < mode_lengths_[i]; j++) {


Add a comment here to explain the use of x ? y : z.

yminchen · 2019-11-04T18:20:03Z

@mposa I've added some documentation and updated code spacing, but I'm not sure if this is the code style you want.

mposa · 2019-11-04T21:48:50Z

Per face to face with Yu-Ming, add an option to fix the seed for the RNG so runtime comparisons are consistent.

mposa

lgtm. Thank you!

**Detail changes**: Dircon: - fix a bug. Use the jacobian that doesn't skip any constraints in the equations of motion - fix bugs in the index of decision variable when adding constraint - pass in surface normal into constructor of dircon_position_data - add surface (e.g. ground) incline feature to dircon_position_data - use linear constraint instead of LorentzConeConstraint for friction cone (to increase speed) - add a utility function to detect if a MBP is quaternion floating based - add unit norm quaternion constraint and also add a slack variable in dynamic constraint Example: - Trajectory optimization of Cassie squating

yminchen added 16 commits October 14, 2019 15:53

Fix dircon bugs; add an example of cassie squating (not tested yet)

baf47f7

The benchmark code for traj opt compiled

dd9b9cf

Fix a bug in skipping constraints (not tested yet)

0b64c78

Debugged dircon. The traj opt has not converged yet

ee395cd

Found standing solution

f144426

Palyed around constraint and decided benchmark example (takes 1min if…

e89456e

… Scale option=2 and taks 15 mins if Scale option=0); add ground incline for contact constraint; clean up code

Add force constraint at collocation point (but commented out for now)…

76200e0

…; need to use fix stance foot position to get better initial guess

Got good initial guess; snopt is able to find a solution (with scale_…

fd1130f

…option = 0) in 2 mins

Fix code style

fb3d1a8

Code cleanup

ff1a471

Code cleanup

b23e222

Little update

cabc045

Clean up the code for pull request

cadcf12

Clean up the code for pull request

96a42ac

Little update

4a50b9f

Merge branch 'improving-dircon' into add-dircon-example

905031c

yminchen requested a review from mposa October 21, 2019 19:57

mposa requested changes Oct 23, 2019

View reviewed changes

yminchen added 5 commits October 25, 2019 17:19

add constraints to avoid leg crossing, fix typo, and update codes bas…

671b50e

…ed on pull request feedback (haven't add normal vector of contact surface yet)

Replace surface roll pitch with surface normal in dircon_position_data

692c7cc

The solving time became 4 mins or so because of the new initial condi…

9ecf23a

…tion

Disable snopt log

37ca226

Play with the traj opt, found solution in 5 mins

575a291

yminchen added 2 commits October 26, 2019 16:48

Clean up code

37ceab8

Much faster convergence without the constraint that prevents leg cros…

4c3217c

…sing

yminchen force-pushed the add-dircon-example branch from f92b24f to 4c3217c Compare October 31, 2019 23:49

yminchen added 3 commits November 1, 2019 15:17

Gitignore .clwb folder for CLion

e055bf2

Update format to follow style guide

315d956

Template QuaternionNormConstraint

72bba6e

yminchen added 6 commits November 3, 2019 11:41

Move quaternion slack varaible to the beginning of the file

6393ba0

Add back constraint which prevents leg crossing

93b4538

Update code to comply with google style

4b62a8b

Comply with google style

88bae99

Merge remote-tracking branch 'upstream/master'

47acd6c

Merge branch 'master' into add-dircon-example

8ce8ddd

yminchen mentioned this pull request Nov 3, 2019

Trajectory Optimization Performance #106

Open

mposa requested changes Nov 4, 2019

View reviewed changes

Update for code style and add comment

feeaf20

mposa approved these changes Nov 5, 2019

View reviewed changes

Add a comment

031176d

yminchen merged commit ea86f21 into master Nov 6, 2019

Add benchmark example for dircon #107

Add benchmark example for dircon #107

Conversation

yminchen commented Oct 21, 2019 • edited Loading

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

yminchen commented Oct 26, 2019 • edited Loading

mposa 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

Choose a reason for hiding this comment

Choose a reason for hiding this comment

yminchen commented Nov 4, 2019 • edited Loading

mposa commented Nov 4, 2019

mposa left a comment

Choose a reason for hiding this comment

yminchen commented Oct 21, 2019 •

edited

Loading

yminchen commented Oct 26, 2019 •

edited

Loading

yminchen commented Nov 4, 2019 •

edited

Loading