Optional robot state initialization

[captain-yoshi]

Ability to set the desired robot state upon startup :

• Optional parameter
• If the size of joints and initial_state does not match
  • A warning is issued
  • The robot state reverts to it's default behavior -> 0.0

hardware_interface:
  sim_control_mode: 0
  joints:
    - shoulder_pan_joint
    - shoulder_lift_joint
    - elbow_joint
    - wrist_1_joint
    - wrist_2_joint
    - wrist_3_joint
  initial_state:
    - 0.0
    - 1.5707
    - 0.0
    - 1.5707
    - 0.0
    - 0.0

I can change the warning print and the ros param key to your liking.