feat(lcmt_generators) : Add standard LCM generators for C3 Output and Contact Descriptions

[Meow404]

Fixes : #11, #36
Support for : DAIRLab/dairlib#376

• Waiting on feat(lcs-factory-system) : Add lcs factory system to C3 repo with examples #9 to be merged
• Waiting on build(c3): Update version for drake and bazel #14 to be merged (soft requirement : to make updates for new drake and bazel version)
• Waiting on feat(core): add c3 plus #22 to be merged

This change is 

[xuanhien070594]

core/configs/solve_options_default.hpp line 3 at r1 (raw file):

#pragma once
#include <string>
const std::string default_solver_options =

I'm confused about why we use yaml string here instead of normal yaml.

[xuanhien070594]

systems/publishers/output_publisher.h line 46 at r1 (raw file):

              c3::lcmt_output* output) const;

  //  void OutputNextC3Input(const drake::systems::Context<double>& context,

Pls remove commented codes.

[xuanhien070594]

systems/publishers/force_publisher.h line 16 at r1 (raw file):

namespace systems {
namespace publishers {
class ContactForcePublisher : public drake::systems::LeafSystem<double> {

Missing documentation

[xuanhien070594]

systems/publishers/force_publisher.cc line 16 at r1 (raw file):

namespace systems {
namespace publishers {
ContactForcePublisher::ContactForcePublisher() {

We'll need some documentation for input ports and output ports.

[xuanhien070594]

systems/publishers/output_publisher.h line 17 at r2 (raw file):

namespace publishers {
/// Converts a OutputVector object to LCM type lcmt_robot_output
class C3OutputPublisher : public drake::systems::LeafSystem<double> {

Pls add documentation.

[xuanhien070594]

systems/publishers/force_publisher.cc line 46 at r2 (raw file):

  MatrixXd J_c = contact_info->first;
  int contact_force_start = solution->lambda_sol_.rows() - J_c.rows();
  bool using_stewart_and_trinkle_model = contact_force_start > 0;

Using the difference in dimension of lambda and number of force basis to determine the contact model is generally not a safe way. Could we store the type of contact model in the C3Solution?

[Meow404]

FYI, building with different drake versions doesnt seem to be an issue so far between dairlib and c3.

[Meow404]

Reviewable status: 0 of 19 files reviewed, 6 unresolved discussions (waiting on @xuanhien070594)

---

systems/publishers/force_publisher.h line 16 at r1 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Missing documentation

Done.

---

systems/publishers/force_publisher.cc line 16 at r1 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

We'll need some documentation for input ports and output ports.

Done.

---

systems/publishers/output_publisher.h line 46 at r1 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Pls remove commented codes.

Done.

---

systems/publishers/output_publisher.h line 17 at r2 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Pls add documentation.

Done.

[xuanhien070594]

lcmtypes/lcmt_forces.lcm line 1 at r3 (raw file):

package c3;

Should the name be lcmt_contact_forces.lcm?

[xuanhien070594]

multibody/geom_geom_collider.cc line 197 at r5 (raw file):

  if (num_friction_directions < 1) {
    // Planar contact: basis is constructed from the contact and planar normals.
    return ComputePlanarForceBasis(-query_result.signed_distance_pair.nhat_BA_W,

Should we add few lines in documentation to tell the user which forces we are calculating (i.e forces that object A acts on object B or object B acts on object A)?

[xuanhien070594]

multibody/geom_geom_collider.cc line 197 at r5 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Should we add few lines in documentation to tell the user which forces we are calculating (i.e forces that object A acts on object B or object B acts on object A)?

Since you're using minus sign here, it seems that we're calculating the forces that object A acting on object B.

[Meow404]

@Meow404 made 1 comment and resolved 5 discussions.
Reviewable status: 14 of 52 files reviewed, 10 unresolved discussions (waiting on alinasarmiento and xuanhien070594).

---

core/c3.cc line 461 at r6 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

I think we should skip storing QP results if QP fails to solve, since failed solves often give extreme large numbers. It is not safe for downstream usage.

What behavior do you think makes sense?
In particular i'm not sure what behavior is expected when the user tried to get a solution

[xuanhien070594]

core/c3.cc line 461 at r6 (raw file):

Previously, Meow404 (Thomas Stephen Felix) wrote…

What behavior do you think makes sense?
In particular i'm not sure what behavior is expected when the user tried to get a solution

I think if the QP fails to give solution, we can set the x solutions to current state of the robot, and u solutions to 0. If doing this, we need a few lines in the document to explain the expected behaviors. What do you think?

[xuanhien070594]

core/c3_options.h line 79 at r7 (raw file):

  std::vector<double> g_lambda;
  std::vector<double> g_u;
  std::vector<double> g_eta_vector;

Sorry, I forgot why do we need g_eta_vector and u_eta_vector.

[xuanhien070594]

multibody/contact_evaluator.h line 141 at r7 (raw file):

  Eigen::Matrix<double, Eigen::Dynamic, 3> CalcForceBasis(
      const drake::systems::Context<T>& context) const override {
    // For planar contact, pass num_friction_directions = 0 to trigger planar

I thought num_friction_directions=1 for planar contact?

[xuanhien070594]

systems/test/generators_test.cc line 371 at r7 (raw file):

}  // namespace test
}  // namespace systems
}  // namespace c3

missing end line

[xuanhien070594]

I only have a few comments. Otherwise,

@xuanhien070594 partially reviewed 50 files and all commit messages, made 1 comment, and resolved 8 discussions.
Reviewable status: all files reviewed, 5 unresolved discussions (waiting on alinasarmiento and Meow404).

[Meow404]

@Meow404 made 4 comments.
Reviewable status: all files reviewed, 5 unresolved discussions (waiting on alinasarmiento and xuanhien070594).

---

core/c3.cc line 461 at r6 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

I think if the QP fails to give solution, we can set the x solutions to current state of the robot, and u solutions to 0. If doing this, we need a few lines in the document to explain the expected behaviors. What do you think?

Done.

---

core/c3_options.h line 79 at r7 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Sorry, I forgot why do we need g_eta_vector and u_eta_vector.

Done.

---

multibody/contact_evaluator.h line 141 at r7 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

I thought num_friction_directions=1 for planar contact?

Planar contact means only the direction normal to contact plane, no friction directions are included.

---

systems/test/generators_test.cc line 371 at r7 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

missing end line

Done.

[xuanhien070594]

@xuanhien070594 reviewed 6 files and all commit messages, made 1 comment, and resolved 5 discussions.
Reviewable status: complete! all files reviewed, all discussions resolved (waiting on alinasarmiento).