HOTFIX correct variables now used for euler-quaternion conversions

[AstroStucky]

Problem discovered in #115. No Jira Ticket available.

Summary of Changes

• The wrong variables were being used to compute the orientation quaternion, this has been fixed.
• Replaced setEuler with setRPY.
  • To explain why: The function setEuler was resulting in quaternions that did not agree with the ones computed from Euler angles given to the action client script for the same action. For example, I could pass to the action client script the exact same set of Euler angles used for the first ArmMoveCartesian in TestArmMoveCartesianGuarded.plx, which are (3.14, 0, 0), but the orientation that resulted from the action client script would be different from the orientation that resulted from the plexil action call. (3.14, 0, 0) should result in the scoop's bottom facing down, and its opening facing away from the lander (+x-direction), but when called by plexil it would result in bottom up and facing toward lander, so it was as if the roll and yaw were being swapped.
  • This looks to be a bug in tf2. setEuler bad parameter names ros/geometry2#190 Another development team worked around it by using setRPY instead.

Test

Make sure you run the following on this branch, and every other repo on the master branch

1. Boot any world and run TestArmMoveCartesian.plx.
   The ArmMoveCartesian should result in the scoop's bottom pointing directly up, and its opening pointing away from the lander. The subsequent ArmMoveCartesian_Q should result in the scoop's bottom pointing directly down, and its opening pointing away from the lander.

2. In the same world run TestArmMoveCartesianGuarded.plx.
   The scoop's bottom should first point down at the ground, then be moved down into the ground and report that the force threshold was breached. It will then raise up high, then back down but will not quite reached the ground resulting in the force threshold not being breached.

[kmdalal]

The variable error was a dumb mistake of mine, but good insight on the use of setRPY.