minjerk.py coefficient calculation not robust to zero value timestamps

[fredsukkar]

branch: master
commit: 039317d

Sending trajectories to the JTAS with any waypoints who's timestamps are zero, causes the arm to perform an erratic and fast movement and then stop its execution (most likely due to a velocity limit violation). The following runtime warnings are reported in the joint_trajectory_action_server node:

[WARN] [1503994476.373382]: /sdk_velocity_joint_trajectory_action_server_right: Trajectory execution Preempted. Stopping execution.
[INFO] [1503994689.669455]: /sdk_velocity_joint_trajectory_action_server_right: Executing requested joint trajectory
/home/planner/sawyer_ws/src/intera_sdk/intera_interface/src/intera_joint_trajectory_action/minjerk.py:124: RuntimeWarning: invalid value encountered in divide
  v1 = d1 / t1
/home/planner/sawyer_ws/src/intera_sdk/intera_interface/src/intera_joint_trajectory_action/minjerk.py:123: RuntimeWarning: invalid value encountered in divide
  v0 = d0 / t0
/home/planner/sawyer_ws/src/intera_sdk/intera_interface/src/intera_joint_trajectory_action/minjerk.py:128: RuntimeWarning: invalid value encountered in divide
  A=(gx-(x+v*t+(a/2.0)*t*t))/(t*t*t);
/home/planner/sawyer_ws/src/intera_sdk/intera_interface/src/intera_joint_trajectory_action/minjerk.py:129: RuntimeWarning: invalid value encountered in divide
  B=(gv-(v+a*t))/(t*t);
/home/planner/sawyer_ws/src/intera_sdk/intera_interface/src/intera_joint_trajectory_action/minjerk.py:130: RuntimeWarning: invalid value encountered in divide
  C=(ga-a)/t;
[WARN] [1503994700.889902]: /sdk_velocity_joint_trajectory_action_server_right: Trajectory execution Preempted. Stopping execution.
[INFO] [1503994700.999374]: /sdk_velocity_joint_trajectory_action_server_right: Executing requested joint trajectory
[WARN] [1503994717.721683]: /sdk_velocity_joint_trajectory_action_server_right: Trajectory execution Preempted. Stopping execution.
[INFO] [1503994717.923978]: /sdk_velocity_joint_trajectory_action_server_right: Executing requested joint trajectory
[WARN] [1503994739.461399]: /sdk_velocity_joint_trajectory_action_server_right: Trajectory execution Preempted. Stopping execution.
[INFO] [1503994739.669233]: /sdk_velocity_joint_trajectory_action_server_right: Executing requested joint trajectory
/home/planner/sawyer_ws/src/intera_sdk/intera_interface/src/intera_joint_trajectory_action/minjerk.py:124: RuntimeWarning: divide by zero encountered in divide
  v1 = d1 / t1
/home/planner/sawyer_ws/src/intera_sdk/intera_interface/src/intera_joint_trajectory_action/minjerk.py:125: RuntimeWarning: invalid value encountered in multiply
  gv = np.where(np.multiply(v0, v1)>=1e-10, 0.5 * ( v0 + v1 ), np.zeros(k)) # 0 + eps
/home/planner/sawyer_ws/src/intera_sdk/intera_interface/src/intera_joint_trajectory_action/minjerk.py:123: RuntimeWarning: divide by zero encountered in divide
  v0 = d0 / t0
/home/planner/sawyer_ws/src/intera_sdk/intera_interface/src/intera_joint_trajectory_action/minjerk.py:128: RuntimeWarning: divide by zero encountered in divide
  A=(gx-(x+v*t+(a/2.0)*t*t))/(t*t*t);
/home/planner/sawyer_ws/src/intera_sdk/intera_interface/src/intera_joint_trajectory_action/minjerk.py:135: RuntimeWarning: invalid value encountered in subtract
  a3=10*A-4*B+0.5*C;
/home/planner/sawyer_ws/src/intera_sdk/intera_interface/src/intera_joint_trajectory_action/minjerk.py:136: RuntimeWarning: invalid value encountered in add
  a4=(-15*A+7*B-C)/t;
/home/planner/sawyer_ws/src/intera_sdk/intera_interface/src/intera_joint_trajectory_action/minjerk.py:137: RuntimeWarning: invalid value encountered in subtract
  a5=(6*A-3*B+0.5*C)/(t*t);
[WARN] [1503994764.494833]: /sdk_velocity_joint_trajectory_action_server_right: Trajectory execution Preempted. Stopping execution.

I believe this should be dealt with on the SDK side, preventing it from propagating through the controllers and causing this erratic behaviour in the arm.