(!) 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 the Arm

Description: This tutorial shows how to move the arm on Mini Maxwell using the simple_arm_server.

Tutorial Level: INTERMEDIATE

Next Tutorial: Grasping an Object

Overview

Mini Maxwell uses the simple_arm_server for controlling the arm. This server allows you to move the arm through a series of poses, as well as allowing you to open and close the gripper to various dimensions. Internally, the simple arm server:

  • Takes a series gripper position in cartesian space (as a ROS action).
  • For each position, computes the inverse kinematics solution to find the joint angles corresponding to the gripper position.
  • Creates a smooth trajectory, and passes it to the drivers to execute.
  • Returns success or failure.

Tucking the Arm

Mini Max's arm can become overheated if left extended for long periods of time. It is recommended that you tuck the arm when not in use, or any time the robot is navigating. To tuck the arm, run the script:

rosrun mini_max_apps tuck_arm.py

This script will move the arm into a configuration that consumes minimal power. It is recommended to untuck the arm before sending additional movements, to avoid jamming against the center of the frame:

rosrun mini_max_apps tuck_arm.py u

Bringup

Once you have started Mini Maxwell, you will still need to bring up the arm control:

roslaunch mini_max_defs simple_arm_server.launch

Moving to a Position

To move the arm, we need to start an action client, construct a request to the simple arm server, and start the action:

   1 #!/usr/bin/env python
   2 
   3 # typical imports
   4 import roslib; roslib.load_manifest('simple_arm_server')
   5 import rospy, actionlib
   6 
   7 # import TF helpers
   8 import tf
   9 from tf.transformations import quaternion_from_euler
  10 
  11 # import MoveArm action messages
  12 from simple_arm_server.msg import *
  13 
  14 # start a node
  15 rospy.init_node('simple_arm_server_test')
  16 
  17 # create an action client
  18 client = actionlib.SimpleActionClient('move_arm', MoveArmAction)
  19 client.wait_for_server()
  20 
  21 # create a new goal
  22 goal = MoveArmGoal()
  23 goal.header.frame_id = "base_link"
  24 
  25 # a goal is comprised of a series of actions
  26 action = ArmAction()
  27 # this action will move the arm to a position
  28 action.type = ArmAction.MOVE_ARM
  29 # assign an xyz position
  30 action.goal.position.x = 0.3
  31 action.goal.position.y = 0.0
  32 action.goal.position.z = 0.2
  33 # assign an orientation, angle the gripper down 
  34 roll = 0.0
  35 pitch = 1.0
  36 yaw = 0.0
  37 # ROS uses quaternions, we'll get one from our Euler angles
  38 q = quaternion_from_euler(roll, pitch, yaw, 'sxyz')
  39 action.goal.orientation.x = q[0]
  40 action.goal.orientation.y = q[1]
  41 action.goal.orientation.z = q[2]
  42 action.goal.orientation.w = q[3]
  43 # move from current position to new one in 3.0 seconds
  44 action.move_time = rospy.Duration(3.0)
  45 
  46 # add our action to our list of motions
  47 goal.motions.append(action)
  48 try:
  49     client.send_goal(goal)
  50     client.wait_for_result()   
  51     print client.get_result()
  52 except rospy.ServiceException, e:
  53     print "Service did not process request: %s"%str(e)

Wiki: mini_max/Tutorials/Moving the Arm (last edited 2011-10-18 19:41:47 by MichaelFerguson)