Motoman FS100: Velocity feedback control

Question

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!

---

Originally posted by akihiko on ROS Answers with karma: 113 on 2017-12-01

Post score: 0

Answer 1

True velocity control is currently not supported by MotoROS (nor motoman_driver).

Position streaming like you're trying to do also tends to not work reliably, as there is a check in the driver that your new trajectory starts at the current state of the robot (to prevent unexpected motions where the robot tries to move to the first point in the trajectory, within essentially 0 sec). Without very strict synchronisation, that is almost impossible to achieve.

There has been some work to add position streaming to motoman_driver, but it hasn't been merged into the main repository. See ros-industrial/motoman#37 and ros-industrial/motoman#88.

Another alternative might be to use wiki/fs100_motoman. This package already implements point streaming. It is rather old though (last commit about 3 years ago), so I'm not sure whether it is compatible with the current version of MotoROS.

Both of these options are rather experimental, so I expect you to have to overcome some obstacles to get this to work.

---

Originally posted by gvdhoorn with karma: 86574 on 2017-12-01

This answer was ACCEPTED on the original site

Post score: 1