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

Hi, sorry for delay in getting back. I tried planning in rviz that is followed by execution. but the second time execution still doesn't work although in terminal it says Execution completed: SUCCEEDED

hooman gravatar image hooman  ( 2019-02-20 11:54:44 -0500 )edit

as an alternative to execution, would you suggest how to load the trajectory slider with a previously planned trajectory (play an old plan again) using the GUI even after planning a new one? to compare the two (old vs new plan)!

hooman gravatar image hooman  ( 2019-02-20 11:58:07 -0500 )edit

You already have made an execution, It means the robot has already moved, and applying the same planning won't work, because the robot now has a different location after the first execution.

kolya_rage gravatar image kolya_rage  ( 2019-02-20 12:34:10 -0500 )edit

yes, but I used the rviz joint_state_publisher to move the robot back to the start position. shouldn't that help with executing it again?

hooman gravatar image hooman  ( 2019-02-20 12:47:08 -0500 )edit

It should. So you saved your joint_state before execution, then published a new one and then executed once again?

kolya_rage gravatar image kolya_rage  ( 2019-02-20 12:50:20 -0500 )edit

Hi, I used the rviz GUI to set the start and goal state, then use the GUI to plan under planning tab which shows execution, and then press execute but nothing happens. Also if I publish the start state where the robot is at the execute gets disabled.

hooman gravatar image hooman  ( 2019-02-26 13:58:00 -0500 )edit

thanks and sorry for my delay in getting back.

hooman gravatar image hooman  ( 2019-02-26 13:58:28 -0500 )edit