Difference in trajectory Execution between MoveIt and Real Robot

edit

Hi, I'm using YuMi with MoveIt (OMPL, RRTstar) python interface through my own script to move ABB YuMi IRB14000, but the visualized trajectory seems to be different from the actual one which has led to a collision.

My setup is : ros-noetic, abb-robot-driver, ros_control: JointTrajectoryController/velocity_controller

Can you please tell me what is the possible cause/s of this odd behavior, and how can I possibly avoid it? thanks in advance.

Note that: I have modified the following files in abb_robot_driver to work with MoveIt

ex3_controllers.yaml

joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 250

egm_state_controller:
  type         : abb_egm_state_controller/EGMStateController
  publish_rate : 250

jnt_traj_vel_controller:
  type: velocity_controllers/JointTrajectoryController
  joints:
    - yumi_robl_joint_1
    - yumi_robl_joint_2
    - yumi_robl_joint_3
    - yumi_robl_joint_4
    - yumi_robl_joint_5
    - yumi_robl_joint_6
    - yumi_robl_joint_7
    - yumi_robr_joint_1
    - yumi_robr_joint_2
    - yumi_robr_joint_3
    - yumi_robr_joint_4
    - yumi_robr_joint_5
    - yumi_robr_joint_6
    - yumi_robr_joint_7
  constraints:
    stopped_velocity_tolerance: 0
  gains:
    yumi_robl_joint_1: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robl_joint_2: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robl_joint_3: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robl_joint_4: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robl_joint_5: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robl_joint_6: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robl_joint_7: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robr_joint_1: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robr_joint_2: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robr_joint_3: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robr_joint_4: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robr_joint_5: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robr_joint_6: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}
    yumi_robr_joint_7: {p: 1.0, i: 0, d: 0.00, i_clamp: 0.1}

ex3_rws_and_egm_yumi_robot.launch

<node pkg="controller_manager" type="spawner" name="stopped" args="--stopped jnt_traj_vel_controller"/>

and here is my launch file:

and my launch file moveit_planning_execution.launch looks like this:

<launch>

  <!-- the "sim" argument controls whether we connect to a Simulated or Real robot -->
  <!--  - if sim=false, a robot_ip argument is required -->
  <arg name="sim" default="false" doc="Use industrial robot simulator instead of real robot" />
  <arg name="robot_ip" unless="$(arg sim)" value="192.168.125.1" doc="IP of controller (only required if not using industrial simulator)" />

  <!-- By default, we do not start a database (it can be large) -->
  <arg name="db" default="false" doc="Start the MoveIt database" />
  <!-- Allow user to specify database location -->
  <arg name="db_path" default="$(find yumi_moveit_config)/default_warehouse_mongo_db" doc="Path to database files" />

  <!-- load the robot_description parameter before launching ROS-I nodes -->
  <include file="$(find yumi_moveit_config)/launch/planning_context.launch" >
    <arg name="load_robot_description" value="true" />
  </include>

  <!-- run the robot simulator and action interface nodes -->
  <group if="$(arg sim)">
    <include file="$(find industrial_robot_simulator)/launch/robot_interface_simulator.launch" />
  </group>

  <!-- run the "real robot" interface nodes -->
  <!--   - this typically includes: robot_state, motion_interface, and joint_trajectory_action nodes -->
  <!--   - replace these ...