Simulation: Exception in controller when it receives HandPosePacketMessage

[Stefan Kohlbrecher]

When sending this on the "/atlas/inputs/ihmc_msgs/HandPosePacketMessage" topic:

#!

robotSide: 0
dataType: 0
referenceFrame: 0
toHomePosition: False
position: 
  x: 0.0
  y: 0.5
  z: 0.0
orientation: 
  x: 0.0
  y: 0.0
  z: 0.0
  w: 1.0
trajectoryTime: 1.0
jointAngles: []

the simulator reacts with:

#!

Exception while running simulation! Stack Trace:
java.lang.RuntimeException: java.util.concurrent.ExecutionException: java.lang.ArrayIndexOutOfBoundsException: 0
    at com.yobotics.simulationconstructionset.robotController.MultiThreadedRobotControllerExecutor.waitAndWriteData(MultiThreadedRobotControllerExecutor.java:65)
    at com.yobotics.simulationconstructionset.robotController.AbstractThreadedRobotController.doControl(AbstractThreadedRobotController.java:67)
    at com.yobotics.simulationconstructionset.Robot.doControllers(Robot.java:439)
    at com.yobotics.simulationconstructionset.Simulator.doControl(Simulator.java:126)
    at com.yobotics.simulationconstructionset.Simulator.simulate(Simulator.java:65)
    at com.yobotics.simulationconstructionset.Simulation.simulate(Simulation.java:393)
    at com.yobotics.simulationconstructionset.SimulationConstructionSet.simulateCycle(SimulationConstructionSet.java:2234)
    at com.yobotics.simulationconstructionset.SimulationConstructionSet.run(SimulationConstructionSet.java:2124)
    at java.lang.Thread.run(Thread.java:745)
Caused by: java.util.concurrent.ExecutionException: java.lang.ArrayIndexOutOfBoundsException: 0
    at java.util.concurrent.FutureTask.report(FutureTask.java:122)
    at java.util.concurrent.FutureTask.get(FutureTask.java:188)
    at com.yobotics.simulationconstructionset.robotController.MultiThreadedRobotControllerExecutor.waitAndWriteData(MultiThreadedRobotControllerExecutor.java:44)
    ... 8 more
Caused by: java.lang.ArrayIndexOutOfBoundsException: 0
    at us.ihmc.commonWalkingControlModules.packetConsumers.DesiredHandPoseProvider.updateFromNewestPacket(DesiredHandPoseProvider.java:94)
    at us.ihmc.commonWalkingControlModules.packetConsumers.DesiredHandPoseProvider.getDesiredHandPose(DesiredHandPoseProvider.java:127)
    at us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.manipulation.ManipulationControlModule.handleHandPoses(ManipulationControlModule.java:163)
    at us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.manipulation.ManipulationControlModule.doControl(ManipulationControlModule.java:136)
    at us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.AbstractHighLevelHumanoidControlPattern.doArmControl(AbstractHighLevelHumanoidControlPattern.java:238)
    at us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.WalkingHighLevelHumanoidController.doMotionControlInternal(WalkingHighLevelHumanoidController.java:1480)
    at us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.WalkingHighLevelHumanoidController.doMotionControl(WalkingHighLevelHumanoidController.java:1463)
    at us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.highLevelStates.AbstractHighLevelHumanoidControlPattern.doAction(AbstractHighLevelHumanoidControlPattern.java:259)
    at us.ihmc.yoUtilities.stateMachines.GenericStateMachine.doAction(GenericStateMachine.java:114)
    at us.ihmc.commonWalkingControlModules.highLevelHumanoidControl.HighLevelHumanoidControllerManager.doControl(HighLevelHumanoidControllerManager.java:103)
    at us.ihmc.commonWalkingControlModules.controllers.RobotControllerUpdatablesAdapter.doControl(RobotControllerUpdatablesAdapter.java:48)
    at com.yobotics.simulationconstructionset.robotController.ModularRobotController.doControl(ModularRobotController.java:25)
    at us.ihmc.darpaRoboticsChallenge.DRCControllerThread.run(DRCControllerThread.java:327)
    at java.util.concurrent.Executors$RunnableAdapter.call(Executors.java:471)
    at java.util.concurrent.FutureTask.run(FutureTask.java:262)
    at java.util.concurrent.ThreadPoolExecutor.runWorker(ThreadPoolExecutor.java:1145)
    at java.util.concurrent.ThreadPoolExecutor$Worker.run(ThreadPoolExecutor.java:615)
    ... 1 more

While I'm not sure the pose defined in my message is reachable, a exception should not happen, no matter what data is supplied. I suspect one might has to fill the joint array even when not using it, haven't tried yet.

[DouglasS]

@Stefan_Kohlbrecher The controller won't throw an exception in the event of an unreachable pose. I think the cause of this is that even though you've specified a Hand Pose data type instead of a joint angle data type, somewhere in the control code we are (erroneously) attempting to access the Joint Angle array which is empty, so the 0 index is an out of bound index. This is indeed a bug in our code. Thanks for the catch. The workaround for now is to just fill the Joint Angle array with six numbers (which even though we seem to be accessing the array, I can verify that the data type is respected when the controller actually attempt to move the arms).

[DouglasS]

Fixed in 0.3.1-alpha