/!\ work in progress

(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Moving an Arm

Description: This tutorial shows step-by-step how to move an ArbotiX-based arm controlled by the simple arm server.

Tutorial Level: INTERMEDIATE

Prerequisites

This tutorial assumes that you have an ArbotiX/Dynamixel-based arm, with a trajectory controller and URDF properly configured.

Overview

The simple arm server allows you to move an arm through a series of goals. The MoveArm request contains a header and an array of ArmAction goals:

Header header
ArmAction[] goals

The header is used to specify the frame_id of the goals, while each ArmAction goal consists of several fields:

byte type                  # move the arm or the gripper?
 
geometry_msgs/Pose goal    # goal for an arm
float64 command            # goal for a gripper

duration move_time

Arm Position Goals

Most goals will move the arm to a position. In this case, we fill out the pose goal and set type to MOVE_ARM, a constant defined in the ArmAction message.

Gripper Goals

Goals can also set the commanded position of the gripper. In this case, we set type equal to MOVE_GRIPPER and set the command field of the request.

Example Code

The multi_waypoint_test.py example in simple_arm_server shows how to move the arm through a small trajectory, picking up a block, and then dropping it in a different location:

   1 import roslib; roslib.load_manifest('simple_arm_server')
   2 import rospy
   3 import sys
   4 
   5 import tf
   6 from tf.transformations import euler_from_quaternion, quaternion_from_euler
   7 
   8 from simple_arm_server.msg import *
   9 from simple_arm_server.srv import * 
  10 
  11 if __name__ == '__main__':
  12     rospy.init_node('simple_arm_server_test')
  13     rospy.wait_for_service('simple_arm_server/move')
  14     move_srv = rospy.ServiceProxy('simple_arm_server/move', MoveArm) 
  15 
  16     req = MoveArmRequest()  
  17     req.header.frame_id = "base_link"
  18  
  19     action = ArmAction()
  20     action.type = ArmAction.MOVE_ARM
  21     action.goal.position.x = 0.2
  22     action.goal.position.y = -0.09
  23     action.goal.position.z = .1
  24     q = quaternion_from_euler(0.0, 1.57, 0.0, 'sxyz')
  25     action.goal.orientation.x = q[0]
  26     action.goal.orientation.y = q[1]
  27     action.goal.orientation.z = q[2]
  28     action.goal.orientation.w = q[3]
  29     action.move_time = rospy.Duration(5.0)
  30     req.goals.append(action)
  31 
  32     action = ArmAction()
  33     action.type = ArmAction.MOVE_GRIPPER
  34     action.command = 0.03
  35     action.move_time = rospy.Duration(1.0)
  36     req.goals.append(action)
  37 
  38     action = ArmAction()
  39     action.type = ArmAction.MOVE_ARM
  40     action.goal.position.x = 0.2
  41     action.goal.position.y = -0.09
  42     action.goal.position.z = .04
  43     q = quaternion_from_euler(0.0, 1.57, 0.0, 'sxyz')
  44     action.goal.orientation.x = q[0]
  45     action.goal.orientation.y = q[1]
  46     action.goal.orientation.z = q[2]
  47     action.goal.orientation.w = q[3]
  48     action.move_time = rospy.Duration(1.0)
  49     req.goals.append(action)
  50 
  51     action = ArmAction()
  52     action.type = ArmAction.MOVE_GRIPPER
  53     action.command = 0.0254
  54     action.move_time = rospy.Duration(2.0)
  55     req.goals.append(action)
  56 
  57     action = ArmAction()
  58     action.type = ArmAction.MOVE_ARM
  59     action.goal.position.x = 0.2
  60     action.goal.position.y = -0.09
  61     action.goal.position.z = .1
  62     q = quaternion_from_euler(0.0, 1.57, 0.0, 'sxyz')
  63     action.goal.orientation.x = q[0]
  64     action.goal.orientation.y = q[1]
  65     action.goal.orientation.z = q[2]
  66     action.goal.orientation.w = q[3]
  67     action.move_time = rospy.Duration(1.0)
  68     req.goals.append(action)
  69 
  70     action = ArmAction()
  71     action.type = ArmAction.MOVE_ARM
  72     action.goal.position.x = 0.2
  73     action.goal.position.y = 0.09
  74     action.goal.position.z = .1
  75     q = quaternion_from_euler(0.0, 1.57, 0.0, 'sxyz')
  76     action.goal.orientation.x = q[0]
  77     action.goal.orientation.y = q[1]
  78     action.goal.orientation.z = q[2]
  79     action.goal.orientation.w = q[3]
  80     action.move_time = rospy.Duration(5.0)
  81     req.goals.append(action)
  82     
  83     action = ArmAction()
  84     action.type = ArmAction.MOVE_GRIPPER
  85     action.command = 0.03
  86     action.move_time = rospy.Duration(2.0)
  87     req.goals.append(action)
  88 
  89     action = ArmAction()
  90     action.type = ArmAction.MOVE_GRIPPER
  91     action.command = 0.0254
  92     action.move_time = rospy.Duration(1.0)
  93     req.goals.append(action)
  94 
  95     try:
  96         r = move_srv(req)
  97         print r
  98     except rospy.ServiceException, e:
  99         print "Service did not process request: %s"%str(e)

Summary

This tutorial has shown how to move the arm through a series of poses. This can be used for simple pick and place, although the server offers no collision checking. Please see the Moving the Arm tutorial for Mini Maxwell for a specific example.

Wiki: simple_arm_server/Tutorials/Moving an Arm (last edited 2014-10-15 22:36:51 by DirkThomas)