MoveIt! Dynamixel movement issues

edit

Hey guys,

this is my second question in here and hopefully the first to get answered.

So I started working with Dynamixel Motors this week. I am trying to move a 5DOF arm with MoveIt!. The problem is that somehow I can't seem to smooth out the motion in any way. I updated the latency timer on the USB-Port to 1ms now the reading are at approx. 110Hz (12.5Hz with the default 16ms on Ubuntu):

    ~$ rostopic hz /motor_states/pan_tilt_port 
subscribed to [/motor_states/pan_tilt_port]
average rate: 111.085
    min: 0.007s max: 0.011s std dev: 0.00060s window: 111
average rate: 108.620
    min: 0.007s max: 0.024s std dev: 0.00153s window: 217
average rate: 108.171
    min: 0.007s max: 0.024s std dev: 0.00131s window: 324
average rate: 108.144
    min: 0.007s max: 0.024s std dev: 0.00118s window: 432
average rate: 108.132
    min: 0.006s max: 0.024s std dev: 0.00117s window: 465

So far so good I guess. The movement with 1m/s velocity seems to be somewhat smooth as seen in the attached graph:

• As you can see, the error is building up over time. This tells me that the controller speed seems to be wrong. I can't however change anything about it or at least I don't know how to.
• The second problem is at the end, where the "goal_pos" already reached the destination and the "current_pos" is not. The velocity at that point is set to 0 all of a sudden as if the reading of the current position is disregarded and the whole process slowy creeps to the desired position.
• Next problem I see is that the velocity is not smoothly updated. Its jumping up and down and seems to have a lot lower frequency than the position which might be the reason the next thing happens.

So as you can see on the graph below the position gets a lot more jumpy when I slow the movement down with velocity scaling. I also enabled the points on velocity so you can see that its in fact getting updated pretty frequently but the actual value doesn't get published as often.

If Anymore information is needed I will gladly supply code or screenshots.

Hope I get the help I need. Thank you guys in advance!

Gilgamesch47

EDIT 1:

Sorry for not supplying any further information.

I will list some stuff here. The first list are all my topics. The custom topics are ARM_controller, jointX_controller and joint_states. The rest are moveIt generated stock ones.

    ~$ rostopic list
/ARM_controller/command
/ARM_controller/follow_joint_trajectory/cancel
/ARM_controller/follow_joint_trajectory/feedback
/ARM_controller/follow_joint_trajectory/goal
/ARM_controller/follow_joint_trajectory/result
/ARM_controller/follow_joint_trajectory/status
/ARM_controller/state
/attached_collision_object
/collision_object
/diagnostics
/display_robot_state
/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status
/joint1_controller/command
/joint1_controller/state
/joint2_controller/command
/joint2_controller/state
/joint3_controller/command
/joint3_controller/state
/joint4_controller/command
/joint4_controller/state
/joint5_controller/command
/joint5_controller/state
/joint_states
/motor_states/pan_tilt_port
/move_group/cancel
/move_group/display_contacts
/move_group/display_cost_sources
/move_group/display_grasp_markers
/move_group/display_planned_path ...