How can I execute a planned trajectory in rviz again?

asked 2019-02-13 12:01:59 -0500

hooman gravatar image

updated 2019-02-13 20:37:43 -0500

I tried executing a successfully planned trajectory several more time using

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
visual_tools.deleteAllMarkers();

visual_tools.loadRemoteControl();
moveit::planning_interface::MoveGroupInterface move_group("manipulator");
moveit::planning_interface::MoveGroupInterface::Plan my_plan;

bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group, static_cast<rviz_visual_tools::colors(0));
visual_tools.trigger();

visual_tools.prompt("Press next to execute the trajectory again in rviz!");
move_group.execute(my_plan);
visual_tools.trigger();
sleep(3.0);

My launch file is

    <launch>
  <arg name="db" default="false" />
  <arg name="debug" default="false" />
  <arg name="limited" default="false"/>
  <include file="$(find ur10_moveit_config)/launch/planning_context.launch">
    <arg name="load_robot_description" value="true"/>
    <arg name="limited" value="$(arg limited)"/>
  </include>
  <node
    pkg="tf"
    type="static_transform_publisher"
    name="base_link_2_fake_world_static" 
    args="0 0 0.8636 3.14159265358 0 0 /fake_world /base_link 30"/>
  <!-- I do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
    <param name="/use_gui" value="true"/> 
    <!-- listen to the my current joint angles topic published-->
    <rosparam param="source_list">["/my_current_joint_states"]</rosparam>
  </node>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
  <!-- Run the main MoveIt executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find ur10_moveit_config)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>  
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>
  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(find ur10_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" value="true"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>
  <include file="$(find ur10_moveit_config)/launch/default_warehouse_db.launch" if="$(arg db)"/>
</launch>

but the robot (ur10) doesn't move in rviz for the second time (after planning and execution for the first time), although in the terminal it says:

Fake execution of trajectory Completed trajectory execution with status SUCCEEDED ...

I would appreciate any tips on this?

thanks

I have followed this tutorial.

I'm using: ROS_VERSION=1 ROS_DISTRO=kinetic Robot type (using move it config)= UR10 and Ubuntu 16.04 LTS

edit retag flag offensive close merge delete

Comments

you didn't execute you trajectory at the first time, you only planned. but then yes - trajectory was executed. Are you using default ur10 launch file? if so, did you set param simulation to true?

kolya_rage gravatar image kolya_rage  ( 2019-02-13 13:47:20 -0500 )edit

I see, you are following a tutorial. Well, the code above is ok, there are no mistakes. Show me your launch file to run the rviz simulation

kolya_rage gravatar image kolya_rage  ( 2019-02-13 13:53:16 -0500 )edit

I just added my launch file. when I do the plan it will execute it and I can see the motion, but I wanted to do it for a second time further down in my code.

hooman gravatar image hooman  ( 2019-02-13 20:38:43 -0500 )edit

try setting fake execution to false

kolya_rage gravatar image kolya_rage  ( 2019-02-14 02:44:44 -0500 )edit

I will try myself the tutorial a bit later

kolya_rage gravatar image kolya_rage  ( 2019-02-14 02:44:57 -0500 )edit

thanks, I set the fake execution to false in the launch file but now it's looking for controllers, saying

[ERROR] Unable to identify any set of controllers that can actuate the specified joints: [ elbow_joint shoulder_lift_joint shoulder_pan_joint wrist_1_joint wrist_2_joint ...
hooman gravatar image hooman  ( 2019-02-14 13:09:27 -0500 )edit

I have a fake controller in the ur10 config folder

controller_list:
  - name: fake_manipulator_controller
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
      - elbow_joint
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
hooman gravatar image hooman  ( 2019-02-14 13:11:13 -0500 )edit

I see that wasn't a good advice for me. so fake execution is actually robot simulation.

kolya_rage gravatar image kolya_rage  ( 2019-02-15 06:40:34 -0500 )edit

well, every thing seems right to me. Does planning execution works in the rviz? I mean if you do planning in rviz moveit interface?

kolya_rage gravatar image kolya_rage  ( 2019-02-15 06:42:29 -0500 )edit