BodyNode::computePotentialEnergy() wrongly consider the origin of body node instead of COM

[yoshito-okada]

If you're not reporting a bug, please use the forum to ask questions.

Bug Report

Please answer the following questions for yourself before reporting a bug.

• I checked the documentation and the forum but found no answer.
• I checked to make sure that this issue has not already been filed.

Environment

Select the following information.

• DART version: main
• OS name and version name(or number): Ubuntu 20.04
• Compiler name and version number: GCC 9.4.0

Expected Behavior

Please describe the behavior you are expecting.

The potential energy of a body node is computed based on not its frame origin but its spatial position of COM.

Current Behavior

What is the current behavior?
If you have trouble in cmake or make, it would be very helpful to build DART with verbose option:
$ cmake .. -DDART_VERBOSE=On
$ make VERBOSE=1
and then dump the results into a gist and share the link to gist here.

BodyNode::computePotentialEnergy() considers the frame origin (i.e. this->getWorldTransform().translation()) instead of COM (i.e. this->getCOM()).

dart/dart/dynamics/BodyNode.cpp

Line 2035 in 189e24d

return -getMass() * getWorldTransform().translation().dot(gravity);

Steps to Reproduce

Please provide detailed steps for reproducing the issue.

1. Prepare a body node the frame origin and COM of which are different.
2. Call BodyNode::computePotentialEnergy().

Code to Reproduce

Please remember that with a working code it's easier to reproduce the bug and it's much faster to fix it.
It would be great to fork this repo and create a regression test using this template.