fixes #1 and applies proper formatting

The first commit fixes issue #1. It has been successfully tested in simulation, with the real robot and in use with the motion retargeting pipeline.

The second commit applies the code format from the ROS C++ code style guide.