moveit: How to change the initial pose for robot in Rviz?

Question

I used 'moveit_setup_assitant' to config my robot, but I cannot find where can I set the robot's initial pose. The question is that the robot's initial pose in simulation environment is not the same as the one in real world. So is there any way to change the initial pose when I launch the robot in 'demo.launch'?

---

Originally posted by xiaozhenghong on ROS Answers with karma: 5 on 2021-04-11

Post score: 0

---

Original comments

Comment by gvdhoorn on 2021-04-12:
I've updated the title to mention MoveIt, as it would appear that's the context in which you asked this question.

Answer 1

Add an entry to fake_controllers.yaml referencing the named poses like this:

initial:
  - group: panda_arm
    pose: ready
  - group: hand
    pose: open

---

Originally posted by fvd with karma: 2180 on 2021-04-12

This answer was ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by xiaozhenghong on 2021-04-12:
thx for your answering! The method works will in the fake_controllers.yaml, but I use ros_controller.yaml as controllers of my robot(in order to get way points from planning, I set fake_execution in demo.launch to be false). And I found the method will not work in this controller file.

    controller_list:
  - name: left_manipulator_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - ljoint1
      - ljoint2
      - ljoint3
      - ljoint4
      - ljoint5
      - ljoint6
      - ljoint7
  - name: right_manipulator_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - rjoint1
      - rjoint2
      - rjoint3
      - rjoint4
      - rjoint5
      - rjoint6
      - rjoint7
initial:
  - group: left_manipulator
    pose: left_original

Comment by fvd on 2021-04-12:
The position of your real-world robot is determined by reality, not the setting in your YAML. If you want the robot to move to a certain pose, you should plan and execute a motion to it.

Also, consider using your_moveit_config_planning_execution.launch instead of demo.launch if you don't use the fake controllers.

Comment by xiaozhenghong on 2021-04-12:
Ok, thx again!

Answer 2

TLDR

Suppose your initial positions is saved in initial_positions.yaml, you can set the initial position before bringing up the robot as follows

initial_positions = os.path.join(
    get_package_share_directory("your_package"),
    "path/to/initial_positions.yaml",
)

moveit_config = (
    MoveItConfigsBuilder("your_robot", package_name="your_robot_package")
    .robot_description(
        file_path="config/your_robot.urdf.xacro",
        mappings={"initial_positions_file": initial_positions},
    )
    .trajectory_execution(file_path="config/moveit_controllers.yaml")
    .planning_pipelines(pipelines=["ompl"])
    .to_moveit_configs()
)

Details

Since you're using MoveItSetupAssistant, it should have generated your_robot.urdf.xacro

Further, I'm assuming you're launching your robot with MoveItConfigsBuilder with something like

moveit_config = (
    MoveItConfigsBuilder("your_robot", package_name="your_robot_package")
    .robot_description(file_path="config/your_robot.urdf.xacro")
    .trajectory_execution(file_path="config/moveit_controllers.yaml")
    .planning_pipelines(pipelines=["ompl"])
    .to_moveit_configs()
)

Note that the robot_description() method allows specifying a mapping parameter. Also notice that the generated your_robot.urdf.xacro has a xacro arg

    <xacro:arg name="initial_positions_file" default="initial_positions.yaml" />

You can specify a mapping to override the default initial_positions.yaml file like so

initial_positions = os.path.join(
    get_package_share_directory("your_package"),
    "path/to/initial_positions.yaml",
)

moveit_config = (
    MoveItConfigsBuilder("your_robot", package_name="your_robot_package")
    .robot_description(
        file_path="config/your_robot.urdf.xacro",
        mappings={"initial_positions_file": initial_positions},
    )
    .trajectory_execution(file_path="config/moveit_controllers.yaml")
    .planning_pipelines(pipelines=["ompl"])
    .to_moveit_configs()
)