Motoman FS100: Velocity feedback control

edit

Hello, I'm controlling Yaskawa Motoman SIA10F (controller: FS100) over ROS. I'd like to do a velocity feedback control (PD control). As the first-step implementation, I made a simple joy stick control: joy stick command is converted to target joint velocities. My code is like:

for each loop (e.g. 40 Hz):
    make a target trajectory consisting of two points:
      point-0 (time=0): current joint positions, current joint velocities
      point-1 (time=dt): target joint positions after dt, target joint velocities
    send the trajectory to /joint_path_command

The current joint velocities are computed internally, because FS100 does not publish joint velocities in the /joint_states topic. dt=1/40. 40 Hz is chosen because the hz of /joint_states is approx. 40.

When executing this code, it did not work, and I encountered errors:

[ERROR] [1511841339.366407562]: Aborting Trajectory.  Failed to send point (#0): Invalid message (3) : Trajectory start position doesn't match current robot position (3011)                                                                                        
[ERROR] [1511841339.621870698]: Trajectory splicing not yet implemented, stopping current motion.
[ERROR] [1511841339.622553123]: Validation failed: Trajectory doesn't start at current position.

How can I implement a velocity feedback control? If there is an example code, please let me know. Thanks!