MoveIt Trajectory not Executed Correctly on Real Robot

asked 2018-09-08 11:06:40 -0500

JulianR gravatar image

updated 2018-09-10 05:02:14 -0500


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.

 - name: boris_control/arm_action_controller
   action_ns: follow_joint_trajectory
   type: FollowJointTrajectory
   default: true
    - 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.

Wrong Execution

4. What I tried:

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

edit retag flag offensive close merge delete



ROS uses radians for everything (REP-103). Do your servo motors use degrees by any chance? Without converting between the two weird things will happen ..

gvdhoorn gravatar image gvdhoorn  ( 2018-09-08 13:22:15 -0500 )edit

Btw; please consider a topic title change. The current title suggests that you don't understand how to "connect" MoveIt to "a real robot", but that is not the case. The steps you describe are exactly what needs to be done. You're actual problem appears to be your robot doing ..

gvdhoorn gravatar image gvdhoorn  ( 2018-09-08 13:24:25 -0500 )edit

.. something that you don't expect. That is something else and it would be good if your topic title reflected that.

Another comment: I see that your robot consists of a closed-loop kinematics structure. That is not supported by URDF, nor MoveIt, so depending on how you modelled things, that may ..

gvdhoorn gravatar image gvdhoorn  ( 2018-09-08 13:24:44 -0500 )edit

.. also cause the issues you are having.

gvdhoorn gravatar image gvdhoorn  ( 2018-09-08 15:00:53 -0500 )edit

Thank you for confirming that I am in the correct way.

My sensor data is in degree,, but I covert them into radian. (degree / 180 * M_PI)

JulianR gravatar image JulianR  ( 2018-09-08 23:01:25 -0500 )edit

Does closed loop kinematic structure mean those two connections between links? If so I tried to mimic those in the urdf, but because moveit does not support mimic joints I ended up "mimicing" those in my joint state publisher, for planning these are still weird which is why I disable collision...

JulianR gravatar image JulianR  ( 2018-09-08 23:03:07 -0500 )edit

Checking between those links and the robot. After execution they are at the correct position though.

JulianR gravatar image JulianR  ( 2018-09-08 23:03:26 -0500 )edit

My sensor data is in degree,, but I covert them into radian. (degree / 180 * M_PI)

and the other way around? For sending commands to your servos?

Do you also convert from radians to degrees?

gvdhoorn gravatar image gvdhoorn  ( 2018-09-09 04:01:49 -0500 )edit