MoveIt Trajectory not Executed Correctly on Real Robot

edit

Hello,

I got a robot arm using an Arduino Mega 2560. It uses 5 Servo motors to actuate the joints and I can read the current joint angle from 5 Sensors as well. In my Arduino Code I am currently able to receive Joy messages to actuate the motors forward or backwards, as well as declaring a goal joint position for each motor to be run to. This robot I want to connect to MoveIt.

It is still not clear to me, what is the proper way to do so, even though I tried for a few weeks now. Currently my problem is that the path I receive is not the one that is shown as planned in Rviz.

What I currently did:

1. Create a joint_states_publisher Node:

The Sensor data from the Arduino is published with Rosserial. This topic is subscribed to by my joint_states_publisher Node, which publishes the message for all angles on the /joint_states topic, as well as publishing the /tf transform.

2. Setup MoveIt:

Created the MoveIt package with the setup assistant manager. Created the controllers.yaml file to let MoveIt be a client in a follow_joint_trajectory Action. The current robot state is displayed in Rviz and I can start planning for Goal Positions.

controller_list:
 - name: boris_control/arm_action_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
   joints:
    - base_rotation_joint
    - upper_arm_joint
    - elbow_joint
    - lower_arm_joint
    - wrist_joint
    - hand_joint
    - finger_joint

3. Created an Action Server Node:

From what I understand I need to implement an Action server that receives the plan from MoveIt, sends it to the Arduino and monitors it's execution.

The Action Server (ActionLib) contains a ExecuteCB method that gets called when a FollowJointTrajectoryGoal message is sent from MoveIt. I am currently sending each way point of the plan at a time, wait for it to be executed and continue with the next point. I am ignoring the velocity, effort and time values sent by MoveIt.

However, the problem is that the planned path does not match the plan I receive. The starting and goal positions are correct and if I only send those, the execution works. But when I want to reach all joint positions from the plan to drive around an object for example, the joint positions in between are not those shown in the plan by Rviz. I get the plan from the goal message in my Action Server like this:

void executeCB(const control_msgs::FollowJointTrajectoryGoalConstPtr &goal) {
        //...
        for (int i = 0; i < goal -> trajectory.points.size(); i++) {
            // Set next joint angles as goal to be reached.
            moveit_command.angle1 = goal -> trajectory.points[i].positions[0];
            // ...
            publisher.publish(moveit_command); // Looping till this is reached...
        }

I logged those positions and it turns out they are not the ones the plan is showing. Here it can be seen that the plan is to move the arm all the way up, but on execution it drives through the collision object only lifting the arm half way.

4. What I tried:

I can connect MoveIt with Gazebo using ros_control loading a joint_state_publisher and an ... (more)