Adds Ability to Clone a RigidBodyTree.

[liangfok]

This fulfills spiral 1 of #4897. The ability to clone RigidBodyTree is necessary to support RigidBodyTree:: DoToAutoDiffXd().

---

This change is 

[liangfok]

@drake-jenkins-bot linux-xenial-clang-ninja-experimental-open-source please.

[liangfok]

+@amcastro-tri for feature review please. Thanks!

---

Review status: 0 of 5 files reviewed at latest revision, all discussions resolved.

---

Comments from Reviewable

[amcastro-tri]

feature review pass done.

---

Reviewed 4 of 5 files at r1.
Review status: 4 of 5 files reviewed at latest revision, 13 unresolved discussions.

---

drake/multibody/rigid_body_tree.cc, line 146 at r1 (raw file):

  // Clones the rigid bodies.
  for (const auto& body : bodies) {
    if (body->get_name() != std::string(RigidBodyTreeConstants::kWorldName)) {

BTW, I would use the id here. Usually you avoid string comparisons in this kind of methods. The id should uniquely identify a body.

---

drake/multibody/rigid_body_tree.cc, line 154 at r1 (raw file):

  for (const auto& original_body : bodies) {
    if (original_body->get_name() ==
        std::string(RigidBodyTreeConstants::kWorldName)) {

BTW, ditto about string checking vs. using id's

---

drake/multibody/rigid_body_tree.cc, line 216 at r1 (raw file):

        clone->get_mutable_body(cloned_frame_body_index);
    DRAKE_DEMAND(cloned_frame_body != nullptr);
    cloned_frame->set_rigid_body(cloned_frame_body);

Is this loop to set the frame's body pointer needed at all? isn't the argument in RigidBodyFrame::Clone() doing that?

---

drake/multibody/rigid_body_tree.cc, line 221 at r1 (raw file):

  for (const auto& actuator : actuators) {
    RigidBody<double>* cloned_body =
        clone->FindBody(actuator.body_->get_name(), "" /* model_name */,

why not actuator.body_->get_model_name()?

---

drake/multibody/rigid_body_tree.cc, line 266 at r1 (raw file):

  if (this->get_num_frames() != other.get_num_frames()) {
    drake::log()->debug(
        "RigidBodyTree::CompareToClone(): num bodies mismatch:\n"

num frames

---

drake/multibody/rigid_body_tree.cc, line 293 at r1 (raw file):

  if (this->get_num_actuators() != other.get_num_actuators()) {
    drake::log()->debug(
        "RigidBodyTree::CompareToClone(): num velocities mismatch:\n"

num actuators

---

drake/multibody/rigid_body_tree.cc, line 315 at r1 (raw file):

  }
  for (int i = 0; i < this->get_num_actuators(); ++i) {
    const RigidBodyActuator* this_actuator = &actuators.at(i);

BTW, probably worth having a RigidBodyActuator::CompareToClone() for consistency.

---

drake/multibody/rigid_body_tree.cc, line 362 at r1 (raw file):

    }
  }
  for (int i = 0; i < static_cast<int>(this->loops.size()); ++i) {

BTW, ditto here about having a RigidBodyLoop::CompareToClone() just for consistency.

---

drake/multibody/rigid_body_tree.cc, line 377 at r1 (raw file):

      return false;
    }
    if (!this_loop->axis_.isApprox(other_loop->axis_,

didn't we agree on using exact comparison for the clones?

---

drake/multibody/rigid_body_tree.h, line 128 at r1 (raw file):

   * *except* for collision and visual elements are cloned.
   *
   * @param[in] model_instance_ids A set of model instance IDs of the model

no param?

---

drake/multibody/test/rigid_body_tree/CMakeLists.txt, line 28 at r1 (raw file):

endif()

drake_add_cc_test(NAME rigid_body_tree_clone_test SIZE small)

BTW, just curiosity, what do these SIZE arguments do, I've never seen them

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 56 at r1 (raw file):

}

// Tests RigidBodyTree::Clone() using Valkyrie

using Prius with LIDAR.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 71 at r1 (raw file):

      "/examples/Valkyrie/urdf/urdf/"
      "valkyrie_A_sim_drake_one_neck_dof_wide_ankle_rom.urdf";
  AddModelInstanceFromUrdfFileToWorld(filename, multibody::joints::kFixed,

BTW, usually we'd use kQuaternion, but ok for a test if that was your intention.

---

Comments from Reviewable

[liangfok]

I addressed all reviewer comments. PTAL. Thanks!

---

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

---

drake/multibody/rigid_body_tree.cc, line 146 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

BTW, I would use the id here. Usually you avoid string comparisons in this kind of methods. The id should uniquely identify a body.

Done.

---

drake/multibody/rigid_body_tree.cc, line 154 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

BTW, ditto about string checking vs. using id's

Done.

---

drake/multibody/rigid_body_tree.cc, line 216 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Is this loop to set the frame's body pointer needed at all? isn't the argument in RigidBodyFrame::Clone() doing that?

Done. Good point.

---

drake/multibody/rigid_body_tree.cc, line 221 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

why not actuator.body_->get_model_name()?

Done.

---

drake/multibody/rigid_body_tree.cc, line 266 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

num frames

Done.

---

drake/multibody/rigid_body_tree.cc, line 293 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

num actuators

Done.

---

drake/multibody/rigid_body_tree.cc, line 315 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

BTW, probably worth having a RigidBodyActuator::CompareToClone() for consistency.

Done.

---

drake/multibody/rigid_body_tree.cc, line 362 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

BTW, ditto here about having a RigidBodyLoop::CompareToClone() just for consistency.

Done.

---

drake/multibody/rigid_body_tree.cc, line 377 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

didn't we agree on using exact comparison for the clones?

Done. (See new code in RigidBodyLoop::CompareToClone()).

---

drake/multibody/rigid_body_tree.h, line 128 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

no param?

Done.

---

drake/multibody/test/rigid_body_tree/CMakeLists.txt, line 28 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

BTW, just curiosity, what do these SIZE arguments do, I've never seen them

They control how much time CI allows the test to run before aborting. See:

drake/cmake/test.cmake

Line 17 in 71f7140

function(drake_compute_test_timeout SIZE SIZE_OUTVAR TIMEOUT_OUTVAR)

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 56 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

using Prius with LIDAR.

Done. Thanks.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 71 at r1 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

BTW, usually we'd use kQuaternion, but ok for a test if that was your intention.

Done (Added explanation).

---

Comments from Reviewable

[amcastro-tri]

Just a few more comments.

---

Reviewed 7 of 7 files at r2.
Review status: 8 of 9 files reviewed at latest revision, 3 unresolved discussions.

---

drake/multibody/rigid_body_loop.cc, line 15 at r2 (raw file):

bool RigidBodyLoop<double>::CompareToClone(const RigidBodyLoop<double>& other)
    const {
  if (!this->frameA_->CompareToClone(*other.frameA_)) {

BTW, for my own education. How is the use of drake::log() better or more convenient that writing to cerr?

---

drake/multibody/rigid_body_tree.cc, line 155 at r2 (raw file):

  for (const auto& original_body : bodies) {
    const int original_body_index =
        this->FindBodyIndex(original_body->get_name(),

BTW, I don't know why you'd have this here. I would just simplify and have a single index, body_id = original_body->get_index() . RigidBody::Clone() ensures the cloned body has the same index so no need to re-check.

---

drake/multibody/rigid_body_tree.cc, line 205 at r2 (raw file):

  for (const auto& actuator : actuators) {
    RigidBody<double>* cloned_body =
        clone->FindBody(actuator.body_->get_name(),

Why instead of using Find (clearly more expensive) not to use get_body(int body_index)? so cloned_body = clone->get_body(body->get_index()). Less typing also

---

drake/multibody/test/rigid_body_tree/CMakeLists.txt, line 28 at r1 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

They control how much time CI allows the test to run before aborting. See:

drake/cmake/test.cmake

Line 17 in 71f7140

function(drake_compute_test_timeout SIZE SIZE_OUTVAR TIMEOUT_OUTVAR)

Cool, thanks!

---

Comments from Reviewable

[liangfok]

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

---

drake/multibody/rigid_body_loop.cc, line 15 at r2 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

BTW, for my own education. How is the use of drake::log() better or more convenient that writing to cerr?

The use of a logging subsystem is advantageous in that you can redirect all log messages by changing just the logging subsystem rather than every call sight. In this case, we disable the log messages so that they incur zero instructions when Drake is built in non-debug mode. As another example, we can modify the logging subsytem to store the log output to a file or even on a remote server. For a full list of features of the particular backend being used in this context, see: https://github.com/gabime/spdlog#features.

The above may also be possible using cerr, though it's likely more difficult to achieve.

---

Comments from Reviewable

[amcastro-tri]

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

---

drake/multibody/rigid_body_loop.cc, line 15 at r2 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

The use of a logging subsystem is advantageous in that you can redirect all log messages by changing just the logging subsystem rather than every call sight. In this case, we disable the log messages so that they incur zero instructions when Drake is built in non-debug mode. As another example, we can modify the logging subsytem to store the log output to a file or even on a remote server. For a full list of features of the particular backend being used in this context, see: https://github.com/gabime/spdlog#features.

The above may also be possible using cerr, though it's likely more difficult to achieve.

Thanks!

---

Comments from Reviewable

[amcastro-tri]

Review status: 4 of 28 files reviewed at latest revision, 5 unresolved discussions.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 195 at r5 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

I originally did that but then I wanted to test two plants, one using an "original" RBT and another containing a "cloned" RBT. That is why I modified it to take a RBT as an input parameter.

I see, I guess it's ok for a unit test.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 277 at r5 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

In this case, both the original and cloned RigidbodyTree objects should be exactly equal to each other, so theoretically it shouldn't matter what dt is used right? After applying the fix in #5110, the unit test now passes. Since it passes, do you think it's still necessary to select a principled dt as described above?

Yes, I do. The period is T = 2 pi sqrt(L/g). With L=0.75 and earth's gravity this gives you about T = 1.7 secs. So with a one millisecond time step you are using about 1700 time steps per period. A little bit of an overkill, but its ok.
For simulation choosing numbers at random is as bad practice as not following the style guide for C++ (actually worst in my opinion).

---

Comments from Reviewable

[sherm1]

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

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 277 at r5 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Yes, I do. The period is T = 2 pi sqrt(L/g). With L=0.75 and earth's gravity this gives you about T = 1.7 secs. So with a one millisecond time step you are using about 1700 time steps per period. A little bit of an overkill, but its ok.
For simulation choosing numbers at random is as bad practice as not following the style guide for C++ (actually worst in my opinion).

BTW, using a variable step integrator seems like a better option than trying to estimate the ideal time step ahead of time. While it may be possible here in general it can't be done in the presence of drastic changes in behavior during a simulation (such as impacts).

---

Comments from Reviewable

[liangfok]

Review status: 4 of 11 files reviewed at latest revision, 4 unresolved discussions.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 277 at r5 (raw file):

Previously, sherm1 (Michael Sherman) wrote…

BTW, using a variable step integrator seems like a better option than trying to estimate the ideal time step ahead of time. While it may be possible here in general it can't be done in the presence of drastic changes in behavior during a simulation (such as impacts).

Done.

---

Comments from Reviewable

[amcastro-tri]

Feature review pass done. Just a few comments but otherwise this is great @liangfok. Thanks.

---

Reviewed 2 of 7 files at r4, 4 of 5 files at r6.
Review status: 10 of 11 files reviewed at latest revision, 9 unresolved discussions.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 29 at r6 (raw file):

namespace drake {

using multibody::CompareToClone;

It seems like this method CompareToClone() is written to be used only within these tests
Should you move to namespace drake::multibody::test::rigid_body_tree::CompareToClone?

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 177 at r6 (raw file):

  // swing period of the pendulum, which is approximated by the equation below:
  //
  //     swing_period = 2 * pi * sqrt(L / g)

BTW, nice! thanks for adding the equation.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 180 at r6 (raw file):

  //
  // where L is the pendulum's center of mass, and g is the acceleration due to
  // gravity. In this case, L = -0.5 and g = -9.81.

Lengths are positive, coordinates can be negative.
Also notice that Pendulum.urdf welds two equal masses, one for "arm" (located at z=-0.5) and one for "arm_com" (located at z=-1.0). Therefore the COM of the compound pendulum is at z=-0.75 and the length you need to use is L=0.75.
Also all quantities need to be provided with units, i.e. L = 0.5 m and g = 9.8 m/s^2

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 184 at r6 (raw file):

  // For more details about the above equation, see the following website:
  //
  // https://en.wikipedia.org/wiki/Pendulum#Period_of_oscillation

BTW, I don't think we should cite wikipedia for this, is not a very scientific source of information. Also this equation is very well known that people usually know it by heart.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 186 at r6 (raw file):

  // https://en.wikipedia.org/wiki/Pendulum#Period_of_oscillation
  //
  const double swing_period = 2 * M_PI * std::sqrt(-0.5 / -9.81);

Use positive quantities. Lengths are positive, the acceleration of gravity is positive as well.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.cc, line 6 at r6 (raw file):

namespace drake {
namespace multibody {

Should you move into drake/multibody/test/rigid_body_tree?

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.h, line 9 at r6 (raw file):

/**
 * Compares a %RigidBodyTree with its clone. Since this method is intended to

Now that it's outside RBT consider updating to: "This method clones the input @p tree and verifies its correctness against the original RigidBodyTree. Since ...`

---

Comments from Reviewable

[liangfok]

Review status: 5 of 11 files reviewed at latest revision, 9 unresolved discussions.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 29 at r6 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

It seems like this method CompareToClone() is written to be used only within these tests
Should you move to namespace drake::multibody::test::rigid_body_tree::CompareToClone?

Done.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 177 at r6 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

BTW, nice! thanks for adding the equation.

You're welcome!

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 180 at r6 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Lengths are positive, coordinates can be negative.
Also notice that Pendulum.urdf welds two equal masses, one for "arm" (located at z=-0.5) and one for "arm_com" (located at z=-1.0). Therefore the COM of the compound pendulum is at z=-0.75 and the length you need to use is L=0.75.
Also all quantities need to be provided with units, i.e. L = 0.5 m and g = 9.8 m/s^2

Done in terms of adding the units. I actually think the center of mass is at Z = -0.5 m. See the screenshot below of Pendulum.urdf loaded into Gazebo:

It shows that the pendlum's arm has a length of 0.75 m and its center of mass is at Z = -0.5 m.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 184 at r6 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

BTW, I don't think we should cite wikipedia for this, is not a very scientific source of information. Also this equation is very well known that people usually know it by heart.

Done.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 186 at r6 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Use positive quantities. Lengths are positive, the acceleration of gravity is positive as well.

Done.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.cc, line 6 at r6 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Should you move into drake/multibody/test/rigid_body_tree?

Done.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_compare_to_clone.h, line 9 at r6 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Now that it's outside RBT consider updating to: "This method clones the input @p tree and verifies its correctness against the original RigidBodyTree. Since ...`

Done.

---

Comments from Reviewable

[amcastro-tri]

Review status: 5 of 11 files reviewed at latest revision, 3 unresolved discussions.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 180 at r6 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

Done in terms of adding the units. I actually think the center of mass is at Z = -0.5 m. See the screenshot below of Pendulum.urdf loaded into Gazebo:

It shows that the pendlum's arm has a length of 0.75 m and its center of mass is at Z = -0.5 m.

I believe Gazebo is just showing the <visual>'s? where are you reading the COM location from? The only way to determine the COM location is from the <inertial> entries and from the mass of each link. Just by looking at the urdf I think the com is at -0.75. I might be wrong, it was a quick read. But the point is that I think Gazebo only shows you visuals. Also notice that the COM of the pendulum depends of both, "arm" and "arm_com", they are welded together into a single pendulum

---

Comments from Reviewable

[liangfok]

Review status: 5 of 11 files reviewed at latest revision, 3 unresolved discussions.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 180 at r6 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

I believe Gazebo is just showing the <visual>'s? where are you reading the COM location from? The only way to determine the COM location is from the <inertial> entries and from the mass of each link. Just by looking at the urdf I think the com is at -0.75. I might be wrong, it was a quick read. But the point is that I think Gazebo only shows you visuals. Also notice that the COM of the pendulum depends of both, "arm" and "arm_com", they are welded together into a single pendulum

Good point. Looking at the URDF, I see the following:

...
  <link name="arm">
      <inertial>
        <origin xyz="0 0 -.5" rpy="0 0 0" />
        <mass value="0.5" />
        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
      </inertial>
      ....
  </link>
  ...
  <link name="arm_com">
    <inertial>
      <origin xyz="0 0 -0.5" rpy="0 0 0" />
      <mass value="0.5"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
   ...
  </link>
  ...
  <joint name="arm_weld" type="fixed">
    <parent link="arm" />
    <child link="arm_com" />
  </joint>
...

Notice that both arm and arm_com have inertial origins at Z = -0.5 and the joint welding them uses the default origin, which I believe is <origin xyz="0 0 0" rpy="0 0 0"/>.

Also, when I use zero the inertias of the Pendulum's world and base_part2 links, RigidBodyTree::centerOfMass() returns Z = -0.5 as described in #5142.

---

Comments from Reviewable

[amcastro-tri]

Review status: 5 of 11 files reviewed at latest revision, 3 unresolved discussions.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 180 at r6 (raw file):

Previously, liangfok (Chien-Liang Fok) wrote…

Good point. Looking at the URDF, I see the following:

...
  <link name="arm">
      <inertial>
        <origin xyz="0 0 -.5" rpy="0 0 0" />
        <mass value="0.5" />
        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
      </inertial>
      ....
  </link>
  ...
  <link name="arm_com">
    <inertial>
      <origin xyz="0 0 -0.5" rpy="0 0 0" />
      <mass value="0.5"/>
      <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
    </inertial>
   ...
  </link>
  ...
  <joint name="arm_weld" type="fixed">
    <parent link="arm" />
    <child link="arm_com" />
  </joint>
...

Notice that both arm and arm_com have inertial origins at Z = -0.5 and the joint welding them uses the default origin, which I believe is <origin xyz="0 0 0" rpy="0 0 0"/>.

Also, when I use zero the inertias of the Pendulum's world and base_part2 links, RigidBodyTree::centerOfMass() returns Z = -0.5 as described in #5142.

Right, the default <origin xyz="0 0 0" rpy="0 0 0"/> means that the joint inboard frame is at the parent body ("arm") body frame, i.e. at z = -0.5.
So that defines the location of the outboard link frame (arm_com) to be located at z = -0.5.
Since link "arm_com" defines with <inertial> the com to be at -0.5 in the body frame (which coincides with the joint's outboard frame per urdf documentation), that means that what this file is saying is that the COM for "arm_com" is at z = -1.0.
Probably we should stop here for the sake of this PR since this is only an order of magnitude computation to determine the time step. However, if RigidBodyTree::centerOfMass() is returning z=-0.5 there might be a bug in there.

---

Comments from Reviewable

[liangfok]

Review status: 5 of 11 files reviewed at latest revision, 3 unresolved discussions.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 180 at r6 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

Right, the default <origin xyz="0 0 0" rpy="0 0 0"/> means that the joint inboard frame is at the parent body ("arm") body frame, i.e. at z = -0.5.
So that defines the location of the outboard link frame (arm_com) to be located at z = -0.5.
Since link "arm_com" defines with <inertial> the com to be at -0.5 in the body frame (which coincides with the joint's outboard frame per urdf documentation), that means that what this file is saying is that the COM for "arm_com" is at z = -1.0.
Probably we should stop here for the sake of this PR since this is only an order of magnitude computation to determine the time step. However, if RigidBodyTree::centerOfMass() is returning z=-0.5 there might be a bug in there.

I'm pretty sure that, in this case, both the "arm" and "arm_com" frames are at <origin xyz="0 0 0" rpy="0 0 0"/>. This is because all of the joints in Pendulum.urdf use the default origin.

---

Comments from Reviewable

[amcastro-tri]

Review status: 5 of 11 files reviewed at latest revision, 3 unresolved discussions.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 180 at r6 (raw file):
You are absolutely right! From the URDF documentation:

This is the transform from the parent link to the child link. The joint is located at the origin of the child link, as shown in the figure above.

I always thought that <origin> specified the inboard frame of the joint in the inboard body frame. However it specifies the pose of the outboard body in the inboard body frame. Good to know. TIL.
Therefore yes, the com of the compound pendulum is at z = 0.5

---

Comments from Reviewable

[liangfok]

Review status: 5 of 11 files reviewed at latest revision, 2 unresolved discussions.

---

drake/multibody/test/rigid_body_tree/rigid_body_tree_clone_test.cc, line 180 at r6 (raw file):

Previously, amcastro-tri (Alejandro Castro) wrote…

You are absolutely right! From the URDF documentation:

This is the transform from the parent link to the child link. The joint is located at the origin of the child link, as shown in the figure above.

I always thought that <origin> specified the inboard frame of the joint in the inboard body frame. However it specifies the pose of the outboard body in the inboard body frame. Good to know. TIL.
Therefore yes, the com of the compound pendulum is at z = 0.5

Excellent. Glad to have this cleared up! We should probably update the documentation of Pendulum.urdf to avoid future confusion: #5156.

---

Comments from Reviewable

[david-german-tri]

Reviewed 3 of 5 files at r6, 5 of 5 files at r7.
Review status: all files reviewed at latest revision, 4 unresolved discussions.

---

drake/multibody/BUILD, line 101 at r7 (raw file):

)

drake_cc_library(

testonly = 1,

---

drake/multibody/BUILD, line 105 at r7 (raw file):

    srcs = ["test/rigid_body_tree/rigid_body_tree_compare_to_clone.cc"],
    hdrs = ["test/rigid_body_tree/rigid_body_tree_compare_to_clone.h"],
    linkstatic = 1,

This is not necessary anymore - please sync past #5155 and remove.

---

drake/multibody/rigid_body_actuator.cc, line 19 at r7 (raw file):

      effort_limit_max_(effort_limit_max) {}

bool RigidBodyActuator::CompareToClone(const RigidBodyActuator& other) const {

Another CompareToClone?

---

drake/multibody/rigid_body_loop.cc, line 13 at r7 (raw file):

template <>
bool RigidBodyLoop<double>::CompareToClone(const RigidBodyLoop<double>& other)

Another CompareToClone?

---

Comments from Reviewable

[liangfok]

@david-german-tri, I addressed your comments. PTAL. Thanks.

---

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

---

drake/multibody/BUILD, line 101 at r7 (raw file):

Previously, david-german-tri (David German) wrote…

testonly = 1,

Done.

---

drake/multibody/BUILD, line 105 at r7 (raw file):

Previously, david-german-tri (David German) wrote…

This is not necessary anymore - please sync past #5155 and remove.

Done.

---

drake/multibody/rigid_body_actuator.cc, line 19 at r7 (raw file):

Previously, david-german-tri (David German) wrote…

Another CompareToClone?

Done.

---

drake/multibody/rigid_body_loop.cc, line 13 at r7 (raw file):

Previously, david-german-tri (David German) wrote…

Another CompareToClone?

Done.

---

Comments from Reviewable

[david-german-tri]

---

Reviewed 11 of 12 files at r8.
Review status: all files reviewed at latest revision, all discussions resolved.

---

Comments from Reviewable