adding new point just befor the final point produce jerkey motion

[k-okada]

This will generate smoother motion, expecting not to produce jerkey motion specially the end of the trajectory
this code is introduced in #289, and @wkentaro said I think this works. why don't you merge this? so may be no longer need this code at some point...

Only tested on the gazebo, need to test on the real robot

[k-okada]

before

after

I tested with following code

(defun straight-motion ()
  (let (av0 av1)
    (setq av0 (send *baxter* :rarm :inverse-kinematics (make-coords :pos #f(900 -500 350) :rpy (float-vector 0 0 0)))
          av1 (send *baxter* :rarm :inverse-kinematics (make-coords :pos #f(900 0 350) :rpy (float-vector 0 0 0)))
          )
    (warning-message 2 "move 1 points (tm = 3000)~%")
    (send *ri* :angle-vector (send *baxter* :reset-pose) 3000)
    (send *ri* :wait-interpolation)
    (unix::sleep 1)
    ;;
    (warning-message 2 "move 1 points (tm = nil)~%")
    (send *ri* :angle-vector av1 3000)
    (send *ri* :wait-interpolation)
    (unix::sleep 1)

    (warning-message 2 "move 2 points (tm = 3000)~%")
    (send *ri* :angle-vector-sequence (list av0 av1) 3000)
    (send *ri* :wait-interpolation)
    (unix::sleep 1)

    (warning-message 2 "move 2 points (tm = nil)~%")
    (send *ri* :angle-vector-sequence (list av0 av1))
    (send *ri* :wait-interpolation)
    (unix::sleep 1)

    (warning-message 2 "move 3 points (tm = 3000)~%")
    (send *ri* :angle-vector-sequence (list av0 (midpoint 0.5 av0 av1) av1) 3000)
    (send *ri* :wait-interpolation)
    (unix::sleep 1)

    (warning-message 2 "move 3 points (tm = nil)~%")
    (send *ri* :angle-vector-sequence (list av0 (midpoint 0.5 av0 av1) av1))
    (send *ri* :wait-interpolation)
    (unix::sleep 1)

    ))

  )

But I think fundamental problem is that they use Bezier function at
https://github.com/RethinkRobotics/baxter_interface/blob/master/src/joint_trajectory_action/joint_trajectory_action.py#L293-L312

[k-okada]

RethinkRobotics/baxter_interface@b38ec25 has been merged since v1.1.1 and we nolongr need to add current position if length of trajectory.points is 1

[k-okada]

if RethinkRobotics/baxter_interface#73 is merged, we can remove

 ;; from v1.1.1 if num_points is 1, it will use current position to the first element of trajectory point https://github.com/RethinkRobotics/baxter_interface/commit/b38ec257fece0711adb260ed6bc161096aa3ecae                                        
     (when (>= (length avs) 2)
       (push (send self :state :potentio-vector :wait-until-update t) avs)
       (push 50 tms))