Dev/sf pr #2712

siyuanfeng-tri · 2016-06-30T20:12:07Z

sfeng wrote a inverse dynamics controller for the valkyrie robot in examples/QPInverseDynamicsForHumanoids using the Optimization.h interface.
It is formulated as a Quadratic Program. The current implementation calls SNOPT explicitly for solution. This needs to be updated when appropriate solvers become available.

This change is

amcastro-tri · 2016-06-30T20:32:51Z

+@ggould-tri since is the platform reviewer for today.
+@naveenoid for feature review.

Review status: 0 of 9 files reviewed at latest revision, 1 unresolved discussion.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 154 [r1] (raw file):

   * body frame
   */
  void _fillKinematics(const std::string& name, Isometry3d& pose, Vector6d& vel,

Follow the cpp guide for naming conventions. E.g: FillKinematics.

Comments from Reviewable

naveenoid · 2016-07-01T13:11:20Z

BTW this is going to take me a bit of time to review. In the meanwhile perhaps you can start fixing the problems Jenkins has found so far on some platforms?

Reviewed 1 of 6 files at r2.
Review status: 1 of 9 files reviewed at latest revision, 1 unresolved discussion, some commit checks failed.

Comments from Reviewable

ggould-tri · 2016-07-01T13:18:42Z

At +4000 lines, this PR is well larger than is practical to review. I expect that it will take until well into next week (ie, after the long weekend) before I have a complete set of comments. I will try to file intermediate comments as I go, but this is going to be a long one.

In the future please try to find ways to break up large PRs like this. Even if you don't reach intermediate PR-able points in your development, you can at least structure your ultimate submission for staged review, for instance by using git rebase -i to create a sanitized history and dropping intermediate branch markers into it.

jwnimmer-tri · 2016-07-01T13:20:11Z

FWIW, it is 1000 lines of code + 3000 lines of data.

siyuanfeng-tri · 2016-07-01T13:31:35Z

Thanks for the inputs.
Majority of the lines come from a urdf file.

Review status: 1 of 9 files reviewed at latest revision, 1 unresolved discussion, some commit checks failed.

Comments from Reviewable

ggould-tri · 2016-07-01T15:22:56Z

Saving my comments so far. Review still in progress.

Reviewed 3 of 6 files at r2.
Review status: 3 of 9 files reviewed at latest revision, 19 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.cpp, line 5 [r2] (raw file):

void HumanoidState::FillKinematics(const std::string& name, Isometry3d& pose,
                                    Vector6d& vel, MatrixXd& J, Vector6d& Jdv,

Whitespace: Indentation does not line up.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.cpp, line 6 [r2] (raw file):

void HumanoidState::FillKinematics(const std::string& name, Isometry3d& pose,
                                    Vector6d& vel, MatrixXd& J, Vector6d& Jdv,
                                    const Vector3d& local_offset) {

Code conventions prefer all input parameters before all output parameters:
https://google.github.io/styleguide/cppguide.html#Function_Parameter_Ordering

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.cpp, line 45 [r2] (raw file):

  // body parts
  FillKinematics(pelv.link_name, pelv.pose, pelv.vel, pelv.J, pelv.Jdv);
  // the fictional contact point is 9cm below the ankle joint

FYI -- Complete sentence; capitalize and add a period.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.cpp, line 54 [r2] (raw file):

  FillKinematics(l_foot_sensor.link_name, l_foot_sensor.pose,
                  l_foot_sensor.vel, l_foot_sensor.J, l_foot_sensor.Jdv,
                  Vector3d(0.0215646, 0.0, -0.051054));

Vector3d(0.0215646, 0.0, -0.051054) seems to be a semantically meaningful constant; please extract it into a named variable.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 58 [r2] (raw file):

  VectorXd trq;  // in the same order as vel, but only has actuated joints

  MatrixXd M;  // inertial matrix

Prefer doxygen style for comments documenting public members. In this case that would be ///< Inertial matrix, for example.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 84 [r2] (raw file):

  Vector6d footFT_w[2];  // wrench rotated to world frame

  explicit HumanoidState(std::unique_ptr<RigidBodyTree> robot_in)

FYI -- Consider moving this logic to the cpp file. This would also let you move the include of Side.h.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 123 [r2] (raw file):

   * @brief Do kinematics and compute useful information based on kinematics and
   * measured force torque information.
   *

FYI The next four items are complete sentences and so should end in periods.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 156 [r2] (raw file):

  void FillKinematics(const std::string& name, Isometry3d& pose, Vector6d& vel,
                      MatrixXd& J, Vector6d& Jdv,
                      const Vector3d& local_offset = Vector3d::Zero());

Can this method be const?

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 156 [r2] (raw file):

  void FillKinematics(const std::string& name, Isometry3d& pose, Vector6d& vel,
                      MatrixXd& J, Vector6d& Jdv,
                      const Vector3d& local_offset = Vector3d::Zero());

Code conventions prefer all input parameters before all output parameters:
https://google.github.io/styleguide/cppguide.html#Function_Parameter_Ordering

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 9 [r2] (raw file):

using namespace drake::solvers;

// some version of this should go in Optimization.h

FYI -- Capitalize and punctuate.

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 39 [r2] (raw file):

  // frame.
  // Note that since S.topRows(6) is zero,
  // tau = M_l * qdd + h_l - J^T_l * lamda, _l means the lower num_torque rows of

long line.

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 57 [r2] (raw file):

  //    + (lambda - lambda_d)^2
  //    + all_kinds_of_body_acceleration_cost_terms
  int num_contacts = 2;

FYI -- Consider 'const' on all of these (per https://google.github.io/styleguide/cppguide.html#Constant_Names changing their names to 'k style' is optional).

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 100 [r2] (raw file):

  ////////////////////////////////////////////////////////////////////
  // set up inequality constraints

Capitalize and punctuate throughout.

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 250 [r2] (raw file):

  SolutionResult result;
  SnoptSolver snopt;
  result = snopt.Solve(prog);

Does this really only work with Snopt or would prog.Solve() work here (which uses Snopt if it is available and falls back to Ipopt/Nlopt if not)?

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 265 [r2] (raw file):

    std::shared_ptr<Constraint> cost = cost_b.constraint();
    cost->eval(VariableList2VectorXd(cost_b.variable_list()), val);
    std::cout << "cost term 0.5 x^T * H * x + h0 * x: " << val.transpose()

Unconditionally writing to cout is usually not appropriate, particularly in the high-frequency loop. Consider conditionalizing this, eg #if !defined(NDEBUG)

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 319 [r2] (raw file):

  // sanity checks,
  // check dynamics, foot not moving, should check

I have no idea what this comment means. Rephrase it?

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 342 [r2] (raw file):

  c = 0.5 * qdd.transpose() * input.w_com * rs.J_com.transpose() * rs.J_com *
          qdd +
      input.w_com * (rs.Jdv_com - input.comdd_d).transpose() * rs.J_com * qdd;

FYI -- This expression (and the similar ones following) are difficult to read. Consider (author's discretion) adding some parens, moving your newlines, or otherwise making the code visually resemble its parse.

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 344 [r2] (raw file):

      input.w_com * (rs.Jdv_com - input.comdd_d).transpose() * rs.J_com * qdd;
  tot = c;
  std::cout << "com cost: " << c << std::endl;

FYI -- I believe this code may be called in a tight loop, and so this output will spam the console. Consider conditionalizing it, eg on !defined(NDEBUG).

Comments from Reviewable

naveenoid · 2016-07-03T09:11:43Z

Reviewed 1 of 6 files at r2, 2 of 5 files at r3.
Review status: 4 of 9 files reviewed at latest revision, 17 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.cpp, line 47 [r3] (raw file):

  // the fictional contact point is 9cm below the ankle joint
  FillKinematics(l_foot.link_name, l_foot.pose, l_foot.vel, l_foot.J,
                  l_foot.Jdv, Vector3d(0, 0, -0.09));

Perhaps you could document or make named variables out of all these constant positions such as the suggestion of @ggould-tri below?

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 18 [r3] (raw file):

  std::string link_name;
  Eigen::Isometry3d pose;
  // task space velocity, or twist of a frame that has the same orientation

Nit : Consider using proper sentences here.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 42 [r3] (raw file):

 * It has kinematic values such as task space velocity of various body parts,
 * some measured contact force / torque information, joint torque, etc.
 */

Consider linking to a diagrammatic representation of the various frames and their locations.

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 9 [r3] (raw file):

using namespace drake::solvers;

// some version of this should go in Optimization.h

This should be a TODO comment https://google.github.io/styleguide/cppguide.html#TODO_Comments

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 249 [r3] (raw file):

  prog.SetInitialGuess(lambda, lambda0);
  SolutionResult result;
  SnoptSolver snopt;

Needs a check to see if the solver is available.

Comments from Reviewable

liangfok · 2016-07-03T12:21:30Z

Review status: 4 of 9 files reviewed at latest revision, 17 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 42 [r3] (raw file):

Previously, naveenoid (Naveen Kuppuswamy) wrote…

Consider linking to a diagrammatic representation of the various frames and their locations.

+1 for a diagrammatic representation. A link to it should ideally be added here: http://drake.mit.edu/design.html

Comments from Reviewable

liangfok · 2016-07-03T12:25:53Z

This PR introduces new files. Should the C++ style guide's rules for file names be enforced?

https://google.github.io/styleguide/cppguide.html#File_Names

Review status: 4 of 9 files reviewed at latest revision, 17 unresolved discussions, some commit checks failed.

Comments from Reviewable

ggould-tri · 2016-07-06T14:27:04Z

+1 to Liang's note above. New files must follow the style guide (underscore_style_name.h) preferred style.

Other than that, and the issues noted below, this all looks good to me. I don't know enough about the subject matter to verify all of the math and names, so I hope feature reviewer @naveenoid who has more experience with humanoid mechanics can verify that in more detail.

Reviewed 1 of 9 files at r1, 1 of 6 files at r2, 5 of 5 files at r3.
Review status: all files reviewed at latest revision, 23 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/RBTUtils.cpp, line 13 [r3] (raw file):

  Vector3d pt = element.transform_to_world.translation();

  // get the body's task space vel

Complete sentence: Capitalize and punctuate.

drake/examples/QPInverseDynamicsForHumanoids/RBTUtils.cpp, line 17 [r3] (raw file):

  v.tail<3>() += v.head<3>().cross(pt);

  // global offset between pt and body

Complete sentence: Capitalize and punctuate.

drake/examples/QPInverseDynamicsForHumanoids/RBTUtils.cpp, line 25 [r3] (raw file):

      H_world_to_pt.translation() - element.transform_to_world.translation();

  // add the linear vel from the body rotation

Complete sentence: Capitalize and punctuate.

drake/examples/QPInverseDynamicsForHumanoids/RBTUtils.cpp, line 47 [r3] (raw file):

    J.template block<SPACE_DIMENSION, 1>(0, *it) = Jg.block<3, 1>(0, col);
    // linear part
    // assume qd = 1, the column is the linear velocity.

Complete sentence: Capitalize.

drake/examples/QPInverseDynamicsForHumanoids/RBTUtils.h, line 8 [r3] (raw file):

using namespace Eigen;

// These should go in RigidBodyTree eventually.

This comment should be marked as TODO(siyuanfeng-tri).

drake/examples/QPInverseDynamicsForHumanoids/test.cpp, line 5 [r3] (raw file):

QPOutput TestGravityCompensation(const HumanoidState& rs) {
  // make controller

Complete sentence: Capitalize and punctuate.
(And elsewhere throughout this file)

Comments from Reviewable

siyuanfeng-tri · 2016-07-06T15:16:07Z

I will fix the names. Thanks guys.

Review status: 1 of 9 files reviewed at latest revision, 24 unresolved discussions.

drake/examples/QPInverseDynamicsForHumanoids/test.cpp, line 5 [r3] (raw file):

Previously, ggould-tri wrote…

Complete sentence: Capitalize and punctuate.
(And elsewhere throughout this file)

Done.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.cpp, line 5 [r2] (raw file):

Previously, ggould-tri wrote…

Whitespace: Indentation does not line up.

Done.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.cpp, line 6 [r2] (raw file):

Previously, ggould-tri wrote…

Code conventions prefer all input parameters before all output parameters:
https://google.github.io/styleguide/cppguide.html#Function_Parameter_Ordering

I want to be able to use a default value of zero for local_offset, that's why it's at the last.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.cpp, line 54 [r2] (raw file):

Previously, ggould-tri wrote…

Vector3d(0.0215646, 0.0, -0.051054) seems to be a semantically meaningful constant; please extract it into a named variable.

Done.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.cpp, line 47 [r3] (raw file):

Previously, naveenoid (Naveen Kuppuswamy) wrote…

Perhaps you could document or make named variables out of all these constant positions such as the suggestion of @ggould-tri below?

Done.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 154 [r1] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Follow the cpp guide for naming conventions. E.g: FillKinematics.

Done.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 58 [r2] (raw file):

Previously, ggould-tri wrote…

Prefer doxygen style for comments documenting public members. In this case that would be ///< Inertial matrix, for example.

Done.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 156 [r2] (raw file):

Previously, ggould-tri wrote…

Code conventions prefer all input parameters before all output parameters:
https://google.github.io/styleguide/cppguide.html#Function_Parameter_Ordering

I put local_offset at the last because I want to use a default argument set to zero, since most calls uses zero offset.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 156 [r2] (raw file):

Previously, ggould-tri wrote…

Can this method be const?

Done.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 18 [r3] (raw file):

Previously, naveenoid (Naveen Kuppuswamy) wrote…

Nit : Consider using proper sentences here.

Done.

drake/examples/QPInverseDynamicsForHumanoids/HumanoidState.h, line 42 [r3] (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

+1 for a diagrammatic representation. A link to it should ideally be added here: http://drake.mit.edu/design.html

Will be useful for the real controller, but that's too much detail for this simple example.

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 39 [r2] (raw file):

Previously, ggould-tri wrote…

long line.

Done.

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 100 [r2] (raw file):

Previously, ggould-tri wrote…

Capitalize and punctuate throughout.

Done.

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 250 [r2] (raw file):

Previously, ggould-tri wrote…

Does this really only work with Snopt or would prog.Solve() work here (which uses Snopt if it is available and falls back to Ipopt/Nlopt if not)?

This shouldn't be using any of these. It should only call the QP solvers, which weren't ready at the time.

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 265 [r2] (raw file):

Previously, ggould-tri wrote…

Unconditionally writing to cout is usually not appropriate, particularly in the high-frequency loop. Consider conditionalizing this, eg #if !defined(NDEBUG)

This is intended as an example of how to get to the cost / constraint terms. They won't be called on the robot.

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 319 [r2] (raw file):

Previously, ggould-tri wrote…

I have no idea what this comment means. Rephrase it?

Done.

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 9 [r3] (raw file):

Previously, naveenoid (Naveen Kuppuswamy) wrote…

This should be a TODO comment https://google.github.io/styleguide/cppguide.html#TODO_Comments

Done.

drake/examples/QPInverseDynamicsForHumanoids/QPController.cpp, line 249 [r3] (raw file):

Previously, naveenoid (Naveen Kuppuswamy) wrote…

Needs a check to see if the solver is available.

Done.

drake/examples/QPInverseDynamicsForHumanoids/RBTUtils.cpp, line 13 [r3] (raw file):

Previously, ggould-tri wrote…

Complete sentence: Capitalize and punctuate.

Done.

drake/examples/QPInverseDynamicsForHumanoids/RBTUtils.cpp, line 17 [r3] (raw file):

Previously, ggould-tri wrote…

Complete sentence: Capitalize and punctuate.

Done.

drake/examples/QPInverseDynamicsForHumanoids/RBTUtils.cpp, line 25 [r3] (raw file):

Previously, ggould-tri wrote…

Complete sentence: Capitalize and punctuate.

Done.

drake/examples/QPInverseDynamicsForHumanoids/RBTUtils.cpp, line 47 [r3] (raw file):

Previously, ggould-tri wrote…

Complete sentence: Capitalize.

Done.

drake/examples/QPInverseDynamicsForHumanoids/RBTUtils.h, line 8 [r3] (raw file):

Previously, ggould-tri wrote…

This comment should be marked as TODO(siyuanfeng-tri).

Done.

Comments from Reviewable

amcastro-tri · 2016-07-06T17:00:03Z

My 2 cents on this

Review status: 1 of 9 files reviewed at latest revision, 30 unresolved discussions.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.cc, line 4 [r4] (raw file):

#include <iostream>

const Vector3d HumanoidState::kFootToContactOffset = Vector3d(0, 0, -0.09);

Why to have this offset to be a static constant? is this exclusive to Valkyrie? why not to have it as a parameter (maybe with default value of 9 cm) that the user of HumanoidRobot can specify?

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.cc, line 5 [r4] (raw file):

const Vector3d HumanoidState::kFootToContactOffset = Vector3d(0, 0, -0.09);
const Vector3d HumanoidState::kFootToSensorOffset =

ditto.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.cc, line 38 [r4] (raw file):

  M = robot->massMatrix(cache);
  eigen_aligned_unordered_map<RigidBody const*, Vector6d> f_ext;

TwistVector defined in common/eigen_types.h.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 9 [r4] (raw file):

/**
 * For all the velocity / acceleration / wrench, the first 3 are always angular,

Is this supposed to be a Doxygen doc? if so, what is it documenting? file, class? take a look at the generated doxygen.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 44 [r4] (raw file):

 * some measured contact force / torque information, joint torque, etc.
 */
class HumanoidState {

There is a very specific concept of "state" in Drake and that will become even stronger with System 2.0. Is "HumanoidState" what we want to call this? @sherm1 or @david-german-tri, what's your take on this?

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 44 [r4] (raw file):

 * some measured contact force / torque information, joint torque, etc.
 */
class HumanoidState {

I don't know if worth at this stage. But none of these files seem to be following the style guide, at least for naming conventions.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 63 [r4] (raw file):

  VectorXd vel;  ///< Velocity in generalized coordinate
  // In the same order as vel, but trq contains only actuated joints.
  VectorXd trq;  ///< Joint torque

avoid using abbreviations like these as per style guide. https://google.github.io/styleguide/cppguide.html#General_Naming_Rules

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 66 [r4] (raw file):

  MatrixXd M;  ///< Inertial matrix
  VectorXd h;  ///< Bias term: M * qdd + h = tau + J^T * lambda

Does you bias term include gravity? or is it just the Coriolis term? (usually denoted by C(q,qdot)?)

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 71 [r4] (raw file):

  Vector3d com;      ///< Center of mass
  Vector3d comd;     ///< Com velocity
  MatrixXd J_com;    ///< Com Jacobian: comd = J_com * v

J_times_v_?

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 72 [r4] (raw file):

  Vector3d comd;     ///< Com velocity
  MatrixXd J_com;    ///< Com Jacobian: comd = J_com * v
  Vector3d Jdv_com;  ///< J_com_dot * v

why not Jdot_times_v_?

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 5 [r4] (raw file):

Vector6d GetTaskSpaceVel(const RigidBodyTree& r,
                         const KinematicsCache<double>& cache,
                         int body_or_frame_id, const Vector3d& local_offset) {

I know this is in other places within RBT but I think we should discourage this API. Why not just overload this method on passing either a RigidBody or a RigidBodyFrame?
like having GetTaskSpaceVel(const RigidBodyTree& r, const KinematicsCache<double>& cache, RigidBody& body, const Vector3d& local_offset) and GetTaskSpaceVel(const RigidBodyTree& r, const KinematicsCache<double>& cache, RigidBodyFrame& frame, const Vector3d& local_offset)

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 7 [r4] (raw file):

                         int body_or_frame_id, const Vector3d& local_offset) {
  Isometry3d H_body_to_frame;
  int body_idx = r.parseBodyOrFrameID(body_or_frame_id, &H_body_to_frame);

This is an expensive way to obtain a body.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 7 [r4] (raw file):

                         int body_or_frame_id, const Vector3d& local_offset) {
  Isometry3d H_body_to_frame;
  int body_idx = r.parseBodyOrFrameID(body_or_frame_id, &H_body_to_frame);

Notice that this method should be marked as deprecated as per Twan's comment in the header description. Volunteer to PR that (in just a single PR)?

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.h, line 8 [r3] (raw file):

Previously, siyuanfeng-tri wrote…

Done.

I would say why not to submit these in a separate (way smaller) PR before getting this PR merged? that way we will avoid code duplication.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.h, line 11 [r4] (raw file):

/**
 * @param local_offset from the body frame origin to the point of interest

Documentation is incomplete not only on parameters but on a description.

Comments from Reviewable

ggould-tri · 2016-07-06T17:55:28Z

Reviewed 14 of 14 files at r4.
Review status: all files reviewed at latest revision, 22 unresolved discussions.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.cc, line 6 [r2] (raw file):

Previously, siyuanfeng-tri wrote…

I want to be able to use a default value of zero for local_offset, that's why it's at the last.

Hm, okay. I think that's well within the range of author's discretion.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 250 [r2] (raw file):

Previously, siyuanfeng-tri wrote…

This shouldn't be using any of these. It should only call the QP solvers, which weren't ready at the time.

OK, I'll unblock this on the assumption that it will be locally rewritten soon. I do still believe however that `prog.Solve()` is the correct entry point in general.

Comments from Reviewable

amcastro-tri · 2016-07-06T18:00:42Z

Review status: all files reviewed at latest revision, 20 unresolved discussions.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 66 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Does you bias term include gravity? or is it just the Coriolis term? (usually denoted by C(q,qdot)?)

h --> bias_term?

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 7 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Notice that this method should be marked as deprecated as per Twan's comment in the header description. Volunteer to PR that (in just a single PR)?

I created issue #2751 to track this issue. Feel free to resolve it!

Comments from Reviewable

ggould-tri · 2016-07-06T18:42:05Z

Reviewed 3 of 14 files at r4, 3 of 3 files at r5.
Review status: all files reviewed at latest revision, 20 unresolved discussions.

Comments from Reviewable

siyuanfeng-tri · 2016-07-06T19:09:32Z

Review status: all files reviewed at latest revision, 21 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.cc, line 4 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Why to have this offset to be a static constant? is this exclusive to Valkyrie? why not to have it as a parameter (maybe with default value of 9 cm) that the user of HumanoidRobot can specify?

It is model dependent, and it should be part of the model file or a parameter file. There are many ways of setting it correctly. Again, I am setting up a slightly more complicated toy example here.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.cc, line 38 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

TwistVector defined in common/eigen_types.h.

Done.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 9 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Is this supposed to be a Doxygen doc? if so, what is it documenting? file, class? take a look at the generated doxygen.

For the file mostly. But also sort of applied to Drake as a whole. I just want to put a note like that somewhere, because I was more accustomed to having the linear part first. So this is a big potential source of bugs for people like me.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 44 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

There is a very specific concept of "state" in Drake and that will become even stronger with System 2.0. Is "HumanoidState" what we want to call this? @sherm1 or @david-german-tri, what's your take on this?

Probably not. My intention for this class is to create a collection of useful variables, and be able to update it through a single function call. I don't want to recompute the Jacobian at the foot many times from the given position and velocity, which are what you call "state". I used the word "state" only because I couldn't come up with a better name. Alternative names are welcomed.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 44 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I don't know if worth at this stage. But none of these files seem to be following the style guide, at least for naming conventions.

A lot of them are kind of obvious for the targeted user. Also, some are related to how they are named in papers.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 63 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

avoid using abbreviations like these as per style guide. https://google.github.io/styleguide/cppguide.html#General_Naming_Rules

Done.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 66 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Does you bias term include gravity? or is it just the Coriolis term? (usually denoted by C(q,qdot)?)

yes

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 71 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

J_times_v_?

This is just the J part, you can compute comd by J \* v

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 250 [r2] (raw file):

Previously, ggould-tri wrote…

OK, I'll unblock this on the assumption that it will be locally rewritten soon. I do still believe however that prog.Solve() is the correct entry point in general.

sounds good.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.h, line 11 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Documentation is incomplete not only on parameters but on a description.

I only commented for the ones that are not self explanatory at first sight. I think the others are clear just from their types. I am not sure what more to add.

Comments from Reviewable

sherm1 · 2016-07-06T20:04:10Z

Review status: all files reviewed at latest revision, 22 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 15 [r4] (raw file):

class BodyOfInterest {
 public:
  std::string name;

Style guide only permits public members in a struct, not a class.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 44 [r4] (raw file):

Previously, siyuanfeng-tri wrote…

Probably not.
My intention for this class is to create a collection of useful variables, and be able to update it through a single function call. I don't want to recompute the Jacobian at the foot many times from the given position and velocity, which are what you call "state".
I used the word "state" only because I couldn't come up with a better name. Alternative names are welcomed.

To avoid confusion, how about something non-committal like `HumanoidInfo`?

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 46 [r4] (raw file):

class HumanoidState {
 public:
  ///< Offset from the foot frame to contact position in the foot frame.

Wrong syntax for doxygen that comes before what it's documenting. The < makes it document the preceding code. Here you just want ///.

Comments from Reviewable

amcastro-tri · 2016-07-06T20:10:33Z

Review status: all files reviewed at latest revision, 23 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 9 [r4] (raw file):

Previously, siyuanfeng-tri wrote…

For the file mostly. But also sort of applied to Drake as a whole.
I just want to put a note like that somewhere, because I was more accustomed to having the linear part first. So this is a big potential source of bugs for people like me.

This class does not have a documentation (all new classes must have one). Why don't you add that clarification to this class' documentation? Also, I created #2752, PTAL and add your thoughts. That was something we talked about with @sherm1 and probably now is a good time to make it happen given that clearly the robotics people need it.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 44 [r4] (raw file):

Previously, siyuanfeng-tri wrote…

A lot of them are kind of obvious for the targeted user. Also, some are related to how they are named in papers.

For many of them it is true, I agree. However not for many others. Eg.: `body_name_to_id` just below.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 63 [r4] (raw file):

Previously, siyuanfeng-tri wrote…

Done.

Not done? `trq` doesn't seem like a standard naming convention. Also consider `generalized_force` instead of the term `torque` since torque is usually a 3D vector, not a scalar.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 66 [r4] (raw file):

Previously, siyuanfeng-tri wrote…

yes

Sorry, I meant why not to call `bias_term` to `h`? I would say we are following the conventions documented in the [underactuated course notes](http://underactuated.csail.mit.edu/underactuated.html?chapter=drake). Also documented [in the code](https://github.com/RobotLocomotion/drake/blob/master/drake/systems/plants/RigidBodyTree.h#L288).

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 71 [r4] (raw file):

Previously, siyuanfeng-tri wrote…

This is just the J part, you can compute comd by J * v

My bad. Consider then updating the comment to `COM Jacobian. The center of mass time derivative can be computed as: comd = J_com * v, where v is the vector of generalized velocities`

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 49 [r5] (raw file):

  double time;

Why are all these variables below public?

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.h, line 11 [r4] (raw file):

Previously, siyuanfeng-tri wrote…

I only commented for the ones that are not self explanatory at first sight. I think the others are clear just from their types. I am not sure what more to add.

As a Doxygen documentation of API exposed to the user the documentation should be self contained. Also the doc is missing a brief description.

Comments from Reviewable

siyuanfeng-tri · 2016-07-07T15:59:46Z

Review status: 3 of 9 files reviewed at latest revision, 22 unresolved discussions.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 15 [r4] (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Style guide only permits public members in a struct, not a class.

Done.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 44 [r4] (raw file):

Previously, sherm1 (Michael Sherman) wrote…

To avoid confusion, how about something non-committal like HumanoidInfo?

Info sounds more like param / config ish things. I changed it to status

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 46 [r4] (raw file):

Previously, sherm1 (Michael Sherman) wrote…

Wrong syntax for doxygen that comes before what it's documenting. The < makes it document the preceding code. Here you just want ///.

Done.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 63 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Not done? trq doesn't seem like a standard naming convention. Also consider generalized_force instead of the term torque since torque is usually a 3D vector, not a scalar.

I named joint_torque

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 66 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Sorry, I meant why not to call bias_term to h? I would say we are following the conventions documented in the underactuated course notes. Also documented in the code.

Done.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 71 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

My bad. Consider then updating the comment to COM Jacobian. The center of mass time derivative can be computed as: comd = J_com * v, where v is the vector of generalized velocities

I think this is fine.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 72 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

why not Jdot_times_v_?

Done.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 49 [r5] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Why are all these variables below public?

Done.

Comments from Reviewable

ggould-tri · 2016-07-08T12:51:01Z

A series of minor style nits (some discretionary) and a couple of possible opportunities for const. Platform pending those items.

Reviewed 4 of 14 files at r4, 8 of 8 files at r6.
Review status: all files reviewed at latest revision, 28 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.h, line 149 [r6] (raw file):

 private:
  std::unique_ptr<RigidBodyTree> robot_;

FYI -- consider const.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.h, line 155 [r6] (raw file):

  std::unordered_map<std::string, int> actuator_name_to_id_;

  double time0_;

FYI This is only set in the ctor and can reasonably be const (although it will have to move to the initializer list).

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 352 [r6] (raw file):

  input.coord_names.resize(r.number_of_velocities());
  for (size_t i = 0; i < r.number_of_velocities(); i++) {
    // strip out the "dot" part from name

FYI -- cappunc.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 362 [r6] (raw file):

  output.coord_names.resize(r.number_of_velocities());
  for (size_t i = 0; i < r.number_of_velocities(); i++) {
    // strip out the "dot" part from name

FYI -- cappunc.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.h, line 11 [r6] (raw file):

 */
struct QPInput {
  // Names for each generalized coordinate.

FYI prefer doxygen style comment (///)

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.h, line 14 [r6] (raw file):

  std::vector<std::string> coord_names;

  // Desired task space accelerations for various body parts.

FYI there is a doxygen style for documenting a group of parameters together. It is author's discretion whether to use it or not but you should at least be aware that it exists if you were not.
///@{ /// [documentation for the group] ... ///@}

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.h, line 43 [r6] (raw file):

 */
struct QPOutput {
  // Names for each generalized coordinate.

FYI Prefer doxygen style.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.h, line 46 [r6] (raw file):

  std::vector<std::string> coord_names;

  // Computed task space accelerations of various body parts.

FYI consider doxygen grouping.

Comments from Reviewable

sherm1 · 2016-07-08T15:04:49Z

Review status: all files reviewed at latest revision, 20 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.h, line 14 [r6] (raw file):

Previously, ggould-tri wrote…

FYI there is a doxygen style for documenting a group of parameters together. It is author's discretion whether to use it or not but you should at least be aware that it exists if you were not.
///@{ /// [documentation for the group] ... ///@}

BTW, I think that has to be preceded by a group name to take effect, like:

///@name        Heading for this group of stuff (shows in doxygen)
///@{
...
///@}

Comments from Reviewable

ggould-tri · 2016-07-11T14:10:15Z

Reviewed 1 of 1 files at r7.
Review status: all files reviewed at latest revision, 20 unresolved discussions.

Comments from Reviewable

ggould-tri · 2016-07-12T20:10:55Z

Um, something in here just blew up the diffs list. I now seem to have 96 files to review.

Review status: 6 of 103 files reviewed at latest revision, 16 unresolved discussions, some commit checks failed.

Comments from Reviewable

jwnimmer-tri · 2016-07-12T20:14:25Z

It was some git mistake. Commits to master got added here, as if they we new in this PR. Github says "This branch has conflicts that must be resolved" so perhaps just wait for that to be fixed first?

naveenoid · 2016-07-13T12:06:15Z

FYI (to be non blocking) clearly seem to be merge artefacts..I propose I will complete feature review here and a new PR is built after feature and platform LGTMs..

Review status: 6 of 103 files reviewed at latest revision, 16 unresolved discussions, some commit checks failed.

Comments from Reviewable

naveenoid · 2016-07-13T12:26:22Z

Review status: 6 of 103 files reviewed at latest revision, 18 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 90 [r14] (raw file):

        -rs.foot(i).J.block(0, 6, 6, num_torque).transpose();
  }
  VectorXd tau0 = rs.bias_term().tail(num_torque);

I suggest a rename of tau0 to tauFixedTerm or something else.

drake/examples/QPInverseDynamicsForHumanoids/test.cc, line 74 [r14] (raw file):

  // These corresponds to a nominal pose for the Valkyrie robot: slightly
  // crouched, arm raised a bit.

Could you potentially include a couple of other robot configurations..just to be sure there are no artefacts due to initialising at a single configuration alone..Something like leaning on 1 foot or the other as well..

Comments from Reviewable

siyuanfeng-tri · 2016-07-13T13:49:53Z

Review status: 4 of 103 files reviewed at latest revision, 18 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 90 [r14] (raw file):

Previously, naveenoid (Naveen Kuppuswamy) wrote…

I suggest a rename of tau0 to tauFixedTerm or something else.

I think tau0 is fine. Many constant terms are named with 0.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 7 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I created issue #2751 to track this issue. Feel free to resolve it!

I have seen the comment there, but I do not understand why this function should be deprecated. It should be a core functionality, shouldn't it?

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.h, line 11 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

As a Doxygen documentation of API exposed to the user the documentation should be self contained. Also the doc is missing a brief description.

Done.

drake/examples/QPInverseDynamicsForHumanoids/test.cc, line 74 [r14] (raw file):

Could you potentially include a couple of other robot configurations..just to be sure there are no artefacts due to initialising at a single configuration alone..Something like leaning on 1 foot or the other as well..
1 HOUR AGO

Done
Good for test coverage. On the other hand, this example will be substantially extended in the future to replace the InstantaneousQPController.cpp when everything is ready. So I will leave that to future work.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 9 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

This class does not have a documentation (all new classes must have one). Why don't you add that clarification to this class' documentation?
Also, I created #2752, PTAL and add your thoughts. That was something we talked about with @sherm1 and probably now is a good time to make it happen given that clearly the robotics people need it.

I will leave it as is for now.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_state.h, line 44 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

For many of them it is true, I agree. However not for many others. Eg.: body_name_to_id just below.

Done.

Comments from Reviewable

jwnimmer-tri · 2016-07-13T15:40:54Z

FYI (to be non blocking) clearly seem to be merge artefacts. I propose I will complete feature review here and a new PR is built after feature and platform LGTMs.

For TRI developers, we should not accept that state of affairs. We should be able to help fix the git history (with a force-push) to be sensible, without throwing it away and starting over.

amcastro-tri · 2016-07-25T19:26:38Z

Review status: 2 of 10 files reviewed at latest revision, 15 unresolved discussions.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.cc, line 8 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Output parameter does not follow style guide.

At the point of the call all of these parameters come from a `BodyOfInterest` class. Wouldn't it be a good idea to just pass your BodyOfInterest? You could also pass the pointer and abide to the style guide. You can always inside unpack things for convenience like in: `Isometry3d& pose = body_of_interest->pose;`

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.cc, line 96 [r19] (raw file):

  }

  cop_ = (cop_w[Side::LEFT] * Fz[Side::LEFT] +

In your example test cop_ is never used (only updated here). However in the update here Fz is zero and we have division by zero in all this block of code. Should you have a check for non-zero Fz for your future usages of Update?

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.h, line 77 [r19] (raw file):

Previously, siyuanfeng-tri wrote…

Do I just remove the @brief tag then? Some of the comments are meaningful.

Yes, that's what I meant. Just remove the `@brief` tag. We have autobrief on which means that the first sentence of your comment automatically becomes the brief descriptions.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.h, line 83 [r19] (raw file):

Previously, siyuanfeng-tri wrote…

opps, good call.

We just decided last Friday on just using the `@` parameters, no backslashes. see here https://docs.google.com/document/d/11sHcSeo7DQX4cQY_ueKhYScA7T5IwZeWPgGKLP5bv4w/edit#heading=h.kaaa9o94fcci

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 26 [r19] (raw file):

int QPController::Control(const HumanoidStatus& rs, const QPInput& input,
                          QPOutput& output) {
  if (!is_qp_input_sane(input)) {

This should throw with an appropriate error message.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 5 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I know this is in other places within RBT but I think we should discourage this API. Why not just overload this method on passing either a RigidBody or a RigidBodyFrame?
like having GetTaskSpaceVel(const RigidBodyTree& r, const KinematicsCache<double>& cache, RigidBody& body, const Vector3d& local_offset) and GetTaskSpaceVel(const RigidBodyTree& r, const KinematicsCache<double>& cache, RigidBodyFrame& frame, const Vector3d& local_offset)

Excellent! please next time do the quick reply the `Done` so I can easily find it and mark it resolved.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 7 [r19] (raw file):

                         const RigidBody& body, const Vector3d& local_offset) {
  const auto& element = cache.getElement(body);
  Vector6d T = element.twist_in_world;

T is actually never used and usually we use the capital T's for transformations. Why not to just declare v directly in this line and avoid the copy below?

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 11 [r19] (raw file):

  // Get the body's task space vel.
  Vector6d v = T;

See above. Avoid copy.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 16 [r19] (raw file):

  // Get the global offset between pt and body.
  Isometry3d H_frame_to_pt(Isometry3d::Identity());
  H_frame_to_pt.translation() = local_offset;

This transformation is from the point "frame" to the body frame? Maybe H_pt_to_body is more appropriate?

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 42 [r19] (raw file):

  for (auto it = v_or_q_indices.begin(); it != v_or_q_indices.end(); ++it) {
    // angular part
    J.template block<SPACE_DIMENSION, 1>(0, *it) = Jg.block<3, 1>(0, col);

When compiling this I get:
warning: ‘SPACE_DIMENSION’ is deprecated (declared at /home/amcastro/Drake/tmp-distros/SiyuansBranch/drake-distro/drake/../drake/util/drakeGeometryUtil.h:28): DRAKE DEPRECATED: Use drake::kSpaceDimension instead. [-Wdeprecated-declarations]

Please change to drake::kSpaceDimension.

drake/examples/QPInverseDynamicsForHumanoids/test.cc, line 70 [r19] (raw file):

Previously, siyuanfeng-tri wrote…

It is v. I will rename it to v. I used to use qd and v interchangeably.

I know! it took me some work to get used to this notation too! (still work in progress).

drake/examples/QPInverseDynamicsForHumanoids/test.cc, line 75 [r19] (raw file):

  // crouched, arm raised a bit.
  q[rs.joint_name_to_id().at("rightHipRoll")] = 0.01;
  q[rs.joint_name_to_id().at("rightHipPitch")] = -0.5432;

body_name_to_id maps from a body name to its RigidBody::body_index. However this is not true for joint_name_to_id which maps from a joint name to its starting position number or dof (this is also implicitly assuming one dof per joint).
Consider then renaming to something like joint_name_to_dof or joint_name_to_position_index.

Comments from Reviewable

amcastro-tri · 2016-07-25T21:56:14Z

Done with review. F2f tomorrow to discuss (mostly for my own education) will help wrapping this up. I also have some suggestions regarding the structure of this code but I will leave that up to you to change in this PR (I am ok if you'd like to do those in this one since this PR is huge already) or in a future one.

Review status: 2 of 10 files reviewed at latest revision, 16 unresolved discussions.

drake/examples/QPInverseDynamicsForHumanoids/test.cc, line 1 [r20] (raw file):

#include "drake/Path.h"

you should call this file something more specific like, gravity_compensation_test.cc and the executable should be called the same (without the cc of course).

drake/examples/QPInverseDynamicsForHumanoids/valkyrie_sim_drake.urdf, line 1 [r19] (raw file):

Previously, siyuanfeng-tri wrote…

I kind of want to make this folder independent of others.

Is there a particular issue we should file here? would your test work with the file in the examples/Valkyrie directory? or did you just place this file here so that the test is self contained?

Comments from Reviewable

amcastro-tri · 2016-07-25T22:18:11Z

Review status: 2 of 10 files reviewed at latest revision, 17 unresolved discussions.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.h, line 28 [r20] (raw file):

  MatrixXd J;
  /// task space Jd * v
  Vector6d Jdot_times_v;

Missing a EIGEN_MAKE_ALIGNED_OPERATOR_NEW. We've observed a 50-50% chance of failure especially in Windows builds (#2683, #2679)

Comments from Reviewable

naveenoid · 2016-07-26T00:38:38Z

Im done with my review..Based on F2F and the remaining topics raised by @amcastro-tri , i give it

Review status: 2 of 10 files reviewed at latest revision, 17 unresolved discussions.

Comments from Reviewable

naveenoid · 2016-07-26T00:39:58Z

Reviewed 5 of 15 files at r4, 1 of 9 files at r6, 1 of 4 files at r14, 1 of 1 files at r19, 8 of 8 files at r20.
Review status: all files reviewed at latest revision, 17 unresolved discussions.

Comments from Reviewable

amcastro-tri · 2016-07-26T15:03:08Z

Review status: all files reviewed at latest revision, 21 unresolved discussions.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 132 [r20] (raw file):

    row_idx++;
  }
  // Fx <= mu * Fz, Fx - mu * Fz <= 0

For my own education here. These constraints are imposed on the x and y components separately like |Fx|<mu*Fz and |Fx|<mu*Fz. If I squared and added them up I would get that sqrt(Fx^2+Fy^2) < sqrt(2)*mu*Fz which does not in general imply that the reaction is within the friction cone. I guess the reason to use these conditions is so that they are linear but, wouldn't we need a factor of sqrt(2)?
Add a comment stating that |Fx|<mu_Fz and |Fy|<mu_Fz only imply sqrt(Fx^2+Fy^2)<sqrt(2)_mu_Fz

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 165 [r20] (raw file):

  // forces at multiple contact positions.
  // Mz <= mu * Fz, Mz - mu * Fz <= 0
  for (int i = 0; i < num_contacts; i++) {

Make explicit in the comment that this is just a toy model to simplify the formulation. State that even the equations are not dimensionally consistent.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 238 [r20] (raw file):

  // CoM term (task space acceleration costs)
  // w * (J * vd + Jdv - comdd_d)^T * (J * vd + Jdv - comdd_d)
  prog.AddQuadraticCost(

Only if you feel strong about it write cost in therms of dimensionless weights like: J=w*(x/L_x)^2

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 12 [r20] (raw file):

  // Get the body's task space vel.
  Vector6d v = T;
  v.tail<3>() += v.head<3>().cross(pt);

Add comment here like "Converting from Plucker vector (Featherstone's spatial vectors) to spatial vector algebra as defined by Abhinandan Jain."

Comments from Reviewable

siyuanfeng-tri · 2016-07-26T22:03:39Z

Review status: all files reviewed at latest revision, 21 unresolved discussions.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.cc, line 8 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

At the point of the call all of these parameters come from a BodyOfInterest class. Wouldn't it be a good idea to just pass your BodyOfInterest? You could also pass the pointer and abide to the style guide. You can always inside unpack things for convenience like in:
Isometry3d& pose = body_of_interest->pose;

I could, but I want to make this function a bit more generic, so it doesn't have to work with BodyOfInterest exclusively.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.cc, line 96 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

In your example test cop_ is never used (only updated here). However in the update here Fz is zero and we have division by zero in all this block of code. Should you have a check for non-zero Fz for your future usages of Update?

It should, you are right. This is a typical failure mode.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.h, line 77 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Yes, that's what I meant. Just remove the @brief tag. We have autobrief on which means that the first sentence of your comment automatically becomes the brief descriptions.

Done.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.h, line 83 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

We just decided last Friday on just using the @ parameters, no backslashes. see here https://docs.google.com/document/d/11sHcSeo7DQX4cQY_ueKhYScA7T5IwZeWPgGKLP5bv4w/edit#heading=h.kaaa9o94fcci

Done.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.h, line 28 [r20] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Missing a EIGEN_MAKE_ALIGNED_OPERATOR_NEW. We've observed a 50-50% chance of failure especially in Windows builds (#2683, #2679)

Done.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 26 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

This should throw with an appropriate error message.

It will be more meaningful in the next iteration.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 132 [r20] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

For my own education here. These constraints are imposed on the x and y components separately like |Fx|<mu*Fz and |Fx|<mu*Fz. If I squared and added them up I would get that sqrt(Fx^2+Fy^2) < sqrt(2)*mu*Fz which does not in general imply that the reaction is within the friction cone. I guess the reason to use these conditions is so that they are linear but, wouldn't we need a factor of sqrt(2)?
Add a comment stating that |Fx|<muFz and |Fy|<muFz only imply sqrt(Fx^2+Fy^2)<sqrt(2)muFz

Done.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 165 [r20] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Make explicit in the comment that this is just a toy model to simplify the formulation. State that even the equations are not dimensionally consistent.

Done.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 238 [r20] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Only if you feel strong about it write cost in therms of dimensionless weights like: J=w*(x/L_x)^2

Sounds like a good idea in the next iteration. I will let it slip this time.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 5 [r4] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Excellent! please next time do the quick reply the Done so I can easily find it and mark it resolved.

Done.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 7 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

T is actually never used and usually we use the capital T's for transformations. Why not to just declare v directly in this line and avoid the copy below?

Done.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 11 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

See above. Avoid copy.

Done.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 16 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

This transformation is from the point "frame" to the body frame? Maybe H_pt_to_body is more appropriate?

Done.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 42 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

When compiling this I get:
warning: ‘SPACE_DIMENSION’ is deprecated (declared at /home/amcastro/Drake/tmp-distros/SiyuansBranch/drake-distro/drake/../drake/util/drakeGeometryUtil.h:28): DRAKE DEPRECATED: Use drake::kSpaceDimension instead. [-Wdeprecated-declarations]

Please change to drake::kSpaceDimension.

I think I have already done that?? I am not sure what's causing the warning.

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 12 [r20] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Add comment here like "Converting from Plucker vector (Featherstone's spatial vectors) to spatial vector algebra as defined by Abhinandan Jain."

Done.

drake/examples/QPInverseDynamicsForHumanoids/test.cc, line 70 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I know! it took me some work to get used to this notation too! (still work in progress).

Done.

drake/examples/QPInverseDynamicsForHumanoids/test.cc, line 75 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

body_name_to_id maps from a body name to its RigidBody::body_index. However this is not true for joint_name_to_id which maps from a joint name to its starting position number or dof (this is also implicitly assuming one dof per joint).
Consider then renaming to something like joint_name_to_dof or joint_name_to_position_index.

Done.

drake/examples/QPInverseDynamicsForHumanoids/test.cc, line 1 [r20] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

you should call this file something more specific like, gravity_compensation_test.cc and the executable should be called the same (without the cc of course).

Done.

drake/examples/QPInverseDynamicsForHumanoids/valkyrie_sim_drake.urdf, line 1 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Is there a particular issue we should file here? would your test work with the file in the examples/Valkyrie directory?
or did you just place this file here so that the test is self contained?

I want to make this folder self contained.

Comments from Reviewable

amcastro-tri · 2016-07-27T13:51:03Z

Almost there! now CI is complaining cause there were some API changes in RBT and you need to do .get_name() instead of just .name or .name().

Review status: 2 of 10 files reviewed at latest revision, 6 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.cc, line 8 [r19] (raw file):

Previously, siyuanfeng-tri wrote…

I could, but I want to make this function a bit more generic, so it doesn't have to work with BodyOfInterest exclusively.

I see. ok, up to you.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.h, line 28 [r20] (raw file):

Previously, siyuanfeng-tri wrote…

Done.

It is not there....

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 26 [r19] (raw file):

Previously, siyuanfeng-tri wrote…

It will be more meaningful in the next iteration.

Ok, agree. Specially since in the next iteration you will take all the initialization stage into the Controller's constructor.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 132 [r20] (raw file):

Previously, siyuanfeng-tri wrote…

Done.

Could you be more explicit by stating that `|Fx|

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 10 [r21] (raw file):

  // Get the body's task space vel.
  // Converting from Plucker vector (Featherstone's spatial vectors) to spatial

Thanks! next time I look at this code I will remember how much pain it took us to understand this the first time :-)

drake/examples/QPInverseDynamicsForHumanoids/valkyrie_sim_drake.urdf, line 14 [r19] (raw file):

Previously, siyuanfeng-tri wrote…

I believe this is auto generated. So I am not going to worry about this.

What do you use to auto generate this file? It could be useful for me in the future :-)

Comments from Reviewable

changed humanoidstate to humnoidstatus added a bit more comments.

rid of that line.

rebased

rigid_body_tree_utils.cc add the explicitly templated version of RigidBodyTree::parseBodyOrFrameID at the end of RigidBodyTree.cpp

added in alejandro's new comments.

siyuanfeng-tri · 2016-07-27T18:56:00Z

Review status: 2 of 10 files reviewed at latest revision, 5 unresolved discussions, some commit checks failed.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.cc, line 8 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I see. ok, up to you.

Done.

drake/examples/QPInverseDynamicsForHumanoids/humanoid_status.h, line 28 [r20] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

It is not there....

opps, I put it in humanoid_status..

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 26 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Ok, agree. Specially since in the next iteration you will take all the initialization stage into the Controller's constructor.

Done.

drake/examples/QPInverseDynamicsForHumanoids/qp_controller.cc, line 132 [r20] (raw file):

Add a comment stating that |Fx|<muFz and |Fy|<muFz only imply sqrt(Fx^2+Fy^2)<sqrt(2)muFz

Quoted 98 lines of code…

              6 hours ago















            amcastro-tri (Alejandro Castro)is:







              Discussing: OK to resolve only if acknowledged

              or OK'd
              last comment.
              (Currently: not OK.)




               Dismiss from
              this discussion
              •
              unresolved discussions.













              You are:




               Following: following new comments, but neutral on resolving.






              Discussing: OK to resolve only if acknowledged

              or OK'd
              last comment.
              (Currently: not OK.)



               Satisfied: OK to resolve any time, and own comments don't require acknowledgement.



               Blocking: not OK to resolve until disposition changed manually.


               Withdrawn: dropped and resolved the discussion before it got started.

                This will resolve and hide the discussion.











              Done


            </details>

drake/examples/QPInverseDynamicsForHumanoids/rigid_body_tree_utils.cc, line 10 [r21] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Thanks! next time I look at this code I will remember how much pain it took us to understand this the first time :-)

Done.

drake/examples/QPInverseDynamicsForHumanoids/valkyrie_sim_drake.urdf, line 14 [r19] (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

What do you use to auto generate this file? It could be useful for me in the future :-)

I don't know how it's generated. I got the model file from one of Russ's student. I was told it's auto generated.

Comments from Reviewable

liangfok · 2016-07-27T18:58:47Z

Review status: 1 of 10 files reviewed at latest revision, 5 unresolved discussions.

drake/examples/QPInverseDynamicsForHumanoids/valkyrie_sim_drake.urdf, line 14 [r19] (raw file):

Previously, siyuanfeng-tri wrote…

I don't know how it's generated. I got the model file from one of Russ's student. I was told it's auto generated.

I bet it was generated from the `.xacro` files available here: https://github.com/NASA-JSC-Robotics/val_description

Comments from Reviewable

amcastro-tri · 2016-07-27T19:05:29Z

Waiting for CI now but otherwise

Review status: 1 of 10 files reviewed at latest revision, all discussions resolved, some commit checks pending.

Comments from Reviewable

amcastro-tri assigned ggould-tri and naveenoid Jun 30, 2016

siyuanfeng-tri added 17 commits July 27, 2016 14:37

adde test

1a9bfa8

added qp estimator, added quadprog..

15778a2

add param interface

1bf635f

add file

1cfc06e

changed many things to struct.

97df866

changed humanoidstate to humnoidstatus added a bit more comments.

clang format

64a70a9

remove more public fields.

8b181c0

take the con.controll call out of assert, otherwise Release build gets

77a18bd

rid of that line.

add comments

6c436f7

rebased

fixed a assert bug..

7b98e33

added more comments

ca5b68d

changed referencing RigidBody by int to RigidBody & in

814be0a

rigid_body_tree_utils.cc add the explicitly templated version of RigidBodyTree::parseBodyOrFrameID at the end of RigidBodyTree.cpp

set to master version

dc203d2

rebase master

6d17359

rebased on new master.

bd5cd27

added in alejandro's new comments.

add more alejandro comments.

3361242

add EIGEN_MAKE_ALIGNED_OPERATOR_NEW to BodyOfInterest

eddb017

siyuanfeng-tri force-pushed the dev/sf_pr branch from 4187527 to eddb017 Compare July 27, 2016 18:55

siyuanfeng-tri merged commit 672b5d8 into RobotLocomotion:master Jul 27, 2016

Dev/sf pr #2712

Dev/sf pr #2712

Conversation

siyuanfeng-tri commented Jun 30, 2016 • edited by david-german-tri Loading

amcastro-tri commented Jun 30, 2016

naveenoid commented Jul 1, 2016

ggould-tri commented Jul 1, 2016

jwnimmer-tri commented Jul 1, 2016

siyuanfeng-tri commented Jul 1, 2016

ggould-tri commented Jul 1, 2016

naveenoid commented Jul 3, 2016

liangfok commented Jul 3, 2016

liangfok commented Jul 3, 2016

ggould-tri commented Jul 6, 2016

siyuanfeng-tri commented Jul 6, 2016

amcastro-tri commented Jul 6, 2016

ggould-tri commented Jul 6, 2016

amcastro-tri commented Jul 6, 2016

ggould-tri commented Jul 6, 2016

siyuanfeng-tri commented Jul 6, 2016

sherm1 commented Jul 6, 2016

amcastro-tri commented Jul 6, 2016

siyuanfeng-tri commented Jul 7, 2016

ggould-tri commented Jul 8, 2016

sherm1 commented Jul 8, 2016

ggould-tri commented Jul 11, 2016

ggould-tri commented Jul 12, 2016

jwnimmer-tri commented Jul 12, 2016

naveenoid commented Jul 13, 2016

naveenoid commented Jul 13, 2016

siyuanfeng-tri commented Jul 13, 2016

jwnimmer-tri commented Jul 13, 2016

amcastro-tri commented Jul 25, 2016

amcastro-tri commented Jul 25, 2016

amcastro-tri commented Jul 25, 2016

naveenoid commented Jul 26, 2016

naveenoid commented Jul 26, 2016

amcastro-tri commented Jul 26, 2016

siyuanfeng-tri commented Jul 26, 2016

amcastro-tri commented Jul 27, 2016

siyuanfeng-tri commented Jul 27, 2016

liangfok commented Jul 27, 2016

amcastro-tri commented Jul 27, 2016

siyuanfeng-tri commented Jun 30, 2016 •

edited by david-german-tri

Loading