In this video we are going to see how to set a sequence of goals for a manipulator robot, using the Python API in order to communicate with MoveIt!.

// RELATED LINKS
▸ Original questions: https://answers.ros.org/question/296994/how-to-set-a-sequence-of-goals-in-moveit/
▸ ROS Development Studio (ROSDS)
▸ Robot Ignite Academy
▸ ROS Manipulation in 5 Days

Step 1. Create a project in ROS Development Studio(ROSDS)

ROSDS helps you follow our tutorial in a fast pace without dealing without setting up an environment locally. If you haven’t had an account yet, you can create a free account here. Let’s create a new project and call it moveit_qa.

Step 2. Use the moveit package

In this tutorial, we have already preconfigured the fetch robot and moveit package for it. If you want to know how to configure the moveit package, please check our ROS Manipulation in 5 days course. Then we create a file called execute_trajectories.py inside the fetch_moveit_config folder with the following content.

#! /usr/bin/env python

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)

robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()    
group = moveit_commander.MoveGroupCommander("arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory)

group_variable_values = group.get_current_joint_values()

group_variable_values[0] = 0
group_variable_values[1] = 0
group_variable_values[3] = -1.5
group_variable_values[5] = 1.5
group.set_joint_value_target(group_variable_values)

plan2 = group.plan()
group.go(wait=True)

rospy.sleep(5)

moveit_commander.roscpp_shutdown()

This script assigns target values to the joint 0, 1, 3, 5 and planning the trajectories to reach these goals.

Then we can launch the fetch robot simulation from Simulations->Fetch.

Before launching the file we just created, let’s use the following command to raise the robot to the fetch position

roslaunch fetch_gazebo_demo move_torso.launch

Then we have to launch the moveit package

roslaunch fetch_moveit_config fetch_planning_execution.launch

Finally, we can run our code(you’ll also need to give it permission before executing it)!

chmod +x execute_trajectories.py
rosrun fetch_moveit_config execute_trajectories.py

The robot should move to the new position now. To execute sequence goal. We modified our script as follows.

...
group.go(wait=true)

group_variable_values[3] = 1.5
group.set_joint_value_target(group_variable_values)

plan2 = group.plan()
group.go(wait=true)

group_variable_values[6] = 1.5
group.set_joint_value_target(group_variable_values)

plan2 = group.plan()
group.go(wait=true)

group_variable_values[3] = 0
group_variable_values[5] = 0
group.set_joint_value_target(group_variable_values)

plan2 = group.plan()
group.go(wait=true)

moveit_commander.roscpp.shutdown()
...

Then we run the script again. The robot should do several movements as we planned.

Want to learn more?

If you want to learn more about the motion planning with moveit package, please check our ROS Manipulation in 5 Days course.

 

Edit by: Tony Huang

---

Feedback

Did you like this video? Do you have questions about what is explained? Whatever the case, please leave a comment on the comments section below, so we can interact and learn from each other.

If you want to learn about other ROS topics, please let us know on the comments area and we will do a video about it.