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 9 comments and resolved 2 discussions.
Reviewable status: 16 of 45 files reviewed, 9 unresolved discussions (waiting on Meow404 and xuanhien070594).

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

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

Done.

---

multibody/lcs_factory.cc line 377 at r5 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Can we reuse the contact results from LCSFactory::ComputeContactJacobian? I think we might want to store a list of GeomGeomCollider inside LCSFactory class, so we can quickly get the contact result without recalculation. What do you think?

I've made the change, but maybe there's a better solution? Currently there does not seem to be much overhead (or calculations) in inotializing the geomgeomcollider.

---

multibody/lcs_factory.cc line 417 at r5 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

I think it is worth to mention somewhere in the documentation of LCSFactory about how we stack the contact forces.

For example, Stewart-Trinkle currently stacks them as [slack_1, slack_2, ..., slack N, normal_1, normal_2, ..., normal_N, tangential_1_1, ..., tangential_1_4, ... tangential_N_1, ..., tangential_N_4]

Done.

---

systems/lcmt_generators/c3_trajectory_generator_config.h line 12 at r5 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Could we add some comments to clarify why do we need start and end?

My understanding is that we need those to extract a portion of state vector.

Done.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

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

When importing C3, there was an issues using the relative path of a file. This was meant to be temporary until I find a better solution.

---

multibody/test/lcs_factory_test.cc line 185 at r5 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

Those tests are commented out. We can remove them if they are no longer needed.

Done.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

Should the name be lcmt_contact_forces.lcm?

Done.`

---

systems/lcmt_generators/contact_force_generator.cc line 63 at r5 (raw file):

Previously, xuanhien070594 (Hien Bui) wrote…

I think we're not setting the force to zero right? We just ignore it.

Done.

---

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

Previously, xuanhien070594 (Hien Bui) wrote…

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?

Done.`