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 "goalpos" already reached the destination and the "currentpos" 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 ARMcontroller, jointXcontroller 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
/move_group/feedback
/move_group/goal
/move_group/monitored_planning_scene
/move_group/motion_plan_request
/move_group/ompl/parameter_descriptions
/move_group/ompl/parameter_updates
/move_group/plan_execution/parameter_descriptions
/move_group/plan_execution/parameter_updates
/move_group/planning_scene_monitor/parameter_descriptions
/move_group/planning_scene_monitor/parameter_updates
/move_group/result
/move_group/sense_for_plan/parameter_descriptions
/move_group/sense_for_plan/parameter_updates
/move_group/status
/move_group/trajectory_execution/parameter_descriptions
/move_group/trajectory_execution/parameter_updates
/pickup/cancel
/pickup/feedback
/pickup/goal
/pickup/result
/pickup/status
/place/cancel
/place/feedback
/place/goal
/place/result
/place/status
/planning_scene
/planning_scene_world
/recognized_object_array
/rosout
/rosout_agg
/rviz_user/motionplanning_planning_scene_monitor/parameter_descriptions
/rviz_user/motionplanning_planning_scene_monitor/parameter_updates
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update
/rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update_full
/tf
/tf_static
/trajectory_execution_event

The nodes are as follows:

~$ rosnode list
/dynamixel_manager
/move_group
/robot_state_publisher
/rosout
/rviz_user
/state_publisher

The modified files are

my custom JointStatePublisher "state_publisher.cpp":

#include <string>
#include <math.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <dynamixel_msgs/MotorStateList.h>
//#include <tf/transform_broadcaster.h>

    // robot state
    double s_pan = 0, s_lift = 0, e_flex = 0, w_flex = 0, grip = 0, s_pan_vel = 0, s_lift_vel = 0, e_flex_vel = 0, w_flex_vel = 0, grip_vel = 0;


double intToRad(int pos_int){
    double buffer = (300*M_PI*pos_int)/(1023.0*180.0);
    return buffer;
}

double intToRadPerSec(int pos_int){
    double buffer = (2*M_PI*0.111*pos_int)/(60.0);
    return buffer;
}

void chatterCallback(const dynamixel_msgs::MotorStateList& msg) {

  // NEED TO CONVERT FROM ENCODER UNITS (INTEGER) TO RAD!
  s_pan = intToRad(msg.motor_states[0].position - 512);
  s_lift = intToRad(msg.motor_states[1].position - 512);
  e_flex = intToRad(msg.motor_states[2].position - 512);
  w_flex = intToRad(msg.motor_states[3].position - 512);
  grip = intToRad(msg.motor_states[4].position - 512);

    // ROS doesnt let me publish vel? -> MotState is speed in 0.111 rpm units
    // [state_publisher-3] process has died [pid 14400, exit code -11 ...
  s_pan_vel = intToRadPerSec(msg.motor_states[0].speed);
  s_lift_vel = intToRadPerSec(msg.motor_states[1].speed);
  e_flex_vel = intToRadPerSec(msg.motor_states[2].speed);
  w_flex_vel = intToRadPerSec(msg.motor_states[3].speed);
  grip_vel = intToRadPerSec(msg.motor_states[4].speed);
}

/*
MOTORSTATELIST
dynamixel_msgs/MotorState[] motor_states
  float64 timestamp
  int32 id
  int32 goal
  int32 position
  int32 error
  int32 speed
  float64 load
  float64 voltage
  int32 temperature
  bool moving


JOINTSTATE
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
string[] name
float64[] position
float64[] velocity
float64[] effort


*/

int main(int argc, char** argv) {
    ros::init(argc, argv, "state_publisher");
    ros::init(argc, argv, "sensor_listener");
    ros::NodeHandle n;
    ros::Publisher joint_pub = n.advertise<sensor_msgs::JointState>("joint_states", 1);
    ros::Subscriber sub = n.subscribe("/motor_states/pan_tilt_port", 1000, chatterCallback);
    //tf::TransformBroadcaster broadcaster;
    ros::Rate loop_rate(5000);

    int count = 0;

    const double degree = M_PI/180.0;

    // message declarations
    sensor_msgs::JointState joint_state;

    while (ros::ok()) {
        // update joint_state
        joint_state.header.stamp = ros::Time::now();
        joint_state.name.resize(5);
        joint_state.position.resize(5);
        joint_state.velocity.resize(5);
        joint_state.name[0] ="arm_shoulder_pan_joint";
        joint_state.position[0] = s_pan;
        joint_state.velocity[0] = s_pan_vel;
        joint_state.name[1] ="arm_shoulder_lift_joint";
        joint_state.position[1] = s_lift;
    joint_state.velocity[1] = s_lift_vel;
        joint_state.name[2] ="arm_elbow_flex_joint";
        joint_state.position[2] = e_flex;
    joint_state.velocity[2] = e_flex_vel;
        joint_state.name[3] ="arm_wrist_flex_joint";
        joint_state.position[3] = w_flex;
    joint_state.velocity[3] = w_flex_vel;
        joint_state.name[4] ="gripper_joint";
        joint_state.position[4] = grip;
        joint_state.velocity[4] = grip_vel;


        // send the joint state
        joint_pub.publish(joint_state);

        ros::spinOnce();

    // fixed Rate
        loop_rate.sleep();
    }


    return 0;
}

My launch files "test_launch.launch":

<?xml version="1.0"?>
<launch>

    <arg name="debug" default="false" />

        <include file="$(find dyn_arm_2)/launch/planning_context.launch">
            <arg name="load_robot_description" value="true"/>
    </include>

    <include file="$(find dyn_arm_2)/launch/launch_sensor_and_joints.launch" />

    <include file="$(find dyn_arm_2)/launch/move_group.launch">
            <arg name="allow_trajectory_execution" value="true"/>
            <arg name="fake_execution" value="false"/>
            <arg name="info" value="true"/>
            <arg name="debug" value="$(arg debug)"/>
    </include>

    <include file="$(find dyn_arm_2)/launch/moveit_rviz.launch">
            <arg name="config" value="true"/>
            <arg name="debug" value="$(arg debug)"/>
    </include>

</launch>

and "launchsensorand_joints.launch":

<?xml version="1.0"?>
<launch>  


  <node name="dynamixel_manager" pkg="dynamixel_controllers" type="controller_manager.py" required="true" output="screen">
    <rosparam>
      namespace: dxl_manager
      serial_ports:
        pan_tilt_port:
          port_name: "/dev/ttyUSB0"
          baud_rate: 1000000
          min_motor_id: 1
          max_motor_id: 5
          update_rate: 5000
    </rosparam>
  </node>

  <node name="state_publisher" pkg="dyn_arm_2" type="state_publisher" />

  <!--node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" /-->

  <!-- Given the published joint states, publish tf for the robot links -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen">
  <!--remap from="/joint_states" to="robot/joint_states" /-->
  <!--param name="publish_frequency" value="100.0" /-->
  </node>

<!-- Start tilt joint controller -->
    <rosparam file="$(find dyn_arm_2)/config/tilt.yaml" command="load"/>
    <node name="controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --port pan_tilt_port
                joint1_controller                       
                joint2_controller
                joint3_controller
                joint4_controller
                joint5_controller
                "
          output="screen"/>

  <!-- Start joints trajectory controller controller -->
    <rosparam file="$(find dyn_arm_2)/config/joints_trajectory_controller.yaml" command="load"/>
    <node name="controller_spawner_meta" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --type=meta
                ARM_controller
                joint1_controller                       
                joint2_controller
                joint3_controller
                joint4_controller
                joint5_controller
               "
          output="screen"/>

</launch>

I also added the industrialtrajectoryfilters/UniformSampleFilter to my ompl planning pipeline to smooth things out but...

Everything else is pretty much stock.

---

Edit 2:

Added pictures instead of links.