How to Move a Simulated Robot Arm to a Goal Using ROS

2-after-moving

In this tutorial, we will learn how to use the ROS actionlib to send goals to a simulated robot arm. By the end of this tutorial, you will be able to build the following:

send-goals-to-arm

Real-World Applications

This project has a number of real-world applications: 


  • Order Fulfillment
  • Factories
  • Warehouses
  • Manufacturing
  • Kitchens
  • and more!

Let’s get started!

Prerequisites

  • You have completed this tutorial where you learned how to create a simulated robot arm.

Create a ROS Package

Let’s begin by creating a ROS package.

In a new terminal window, move to the src (source) folder of your workspace.

cd ~/catkin_ws/src

Now create the package.

catkin_create_pkg arm_to_goal actionlib roscpp rospy std_msgs
cd ~/catkin_ws/
catkin_make --only-pkg-with-deps arm_to_goal

Write the Code

Open a new terminal window.

Move to your package.

roscd arm_to_goal

Create a folder for the Python script we will write.

mkdir scripts

Move inside that folder.

cd scripts

Create a program.

gedit send_goal_to_arm.py

Add the following code:

#!/usr/bin/env python3

# Author: Automatic Addison https://automaticaddison.com
# Description: Uses the ROS action lib to move a robotic arm to a goal location

# Import the necessary libraries
from __future__ import print_function # Printing
import rospy # Python client library
import actionlib # ROS action library
from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal # Controller messages
from std_msgs.msg import Float64 # 64-bit floating point numbers
from trajectory_msgs.msg import JointTrajectoryPoint # Robot trajectories


def move_robot_arm(joint_values):
  """
  Function to move the robot arm to desired joint angles.
  :param: joint_values A list of desired angles for the joints of a robot arm 
  """
  # Create the SimpleActionClient, passing the type of the action to the constructor
  arm_client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)

  # Wait for the server to start up and start listening for goals.
  arm_client.wait_for_server()
	
  # Create a new goal to send to the Action Server
  arm_goal = FollowJointTrajectoryGoal()

  # Store the names of each joint of the robot arm
  arm_goal.trajectory.joint_names = ['arm_base_joint', 'shoulder_joint','bottom_wrist_joint' ,'elbow_joint', 'top_wrist_joint']
  
  # Create a trajectory point  	
  point = JointTrajectoryPoint()

  # Store the desired joint values
  point.positions = joint_values

  # Set the time it should in seconds take to move the arm to the desired joint angles
  point.time_from_start = rospy.Duration(3)

  # Add the desired joint values to the goal
  arm_goal.trajectory.points.append(point)

  # Define timeout values
  exec_timeout = rospy.Duration(10)
  prmpt_timeout = rospy.Duration(5)

  # Send a goal to the ActionServer and wait for the server to finish performing the action
  arm_client.send_goal_and_wait(arm_goal, exec_timeout, prmpt_timeout)


if __name__ == '__main__':
  """
  Main method.
  """
  try:
    # Initialize a rospy node so that the SimpleActionClient can
    # publish and subscribe over ROS.
    rospy.init_node('send_goal_to_arm_py')

    # Move the joints of the robot arm to the desired angles in radians
    move_robot_arm([-0.1, 0.5, 0.02, 0, 0])

    print("Robotic arm has successfully reached the goal!")
    
  except rospy.ROSInterruptException:
    print("Program interrupted before completion.", file=sys.stderr)

Change the permissions.

chmod +x send_goal_to_arm.py

Compile the package.

cd ~/catkin_ws/
catkin_make --only-pkg-with-deps arm_to_goal

Run the Code

Open a new terminal window, and type:

cd ~/catkin_ws/
roslaunch mobile_manipulator_body mobile_manipulator_gazebo.launch
1-robot-mobile-manipulator

Open a new terminal window, and type the following command to move the robot to the goal location:

rosrun arm_to_goal send_goal_to_arm.py
2-after-moving-1
3-action-completed

That’s it! Keep building!