Motoman FS100: Velocity feedback control
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!