Using Named Poses

Armer provides the ability to save and load poses for any arm.

Adding a Named Pose

Simply navigate a manipulator into any pose by any means and call the service /arm/set_named_pose. The pose is saved locally to a config file in YAML format and automatically loaded each time Armer is launched.

The service can be summoned via Python:

from armer_msgs.srv import AddNamedPose
import rospy

rospy.init_node('armer_example', disable_signals=True)
add_pose_service = rospy.ServiceProxy('/arm/set_named_pose', AddNamedPose)

named_pose="my_pose"
add_pose_service(named_pose, True)

Or via bash command line:

rosservice call /arm/set_named_pose "pose_name: {DESIRED_POSE_NAME}"

The pose is now saved and the robot can navigate to the saved pose by making a request to the action server.

Moving to a Named Pose

The Armer interface uses a ROS action server to listen for requests to move to a named pose.

Requests to the ROS action server can be made by creating a ROS action client in Python. This client is used to send requests to the action server.

This example shows a request to move to named pose my_pose:

from armer_msgs.msg import MoveToNamedPoseAction, MoveToNamedPoseGoal
import actionlib
import rospy

rospy.init_node('armer_example', disable_signals=True)

named_pose="my_pose"

servo_cli = actionlib.SimpleActionClient('/arm/joint/named', MoveToNamedPoseAction)
servo_cli.wait_for_server()

goal = MoveToNamedPoseGoal()
goal.pose_name=named_pose

# Send goal and wait for it to finish
servo_cli.send_goal(goal)
servo_cli.wait_for_result()
servo_cli.get_result()

Removing a Named Pose

Named poses can be removed via the /arm/remove_named_pose service. This can be called via Python:

from armer_msgs.srv import RemoveNamedPose
import rospy

rospy.init_node('armer_example', disable_signals=True)
remove_pose_service = rospy.ServiceProxy('/arm/remove_named_pose', RemoveNamedPose)

named_pose="my_pose"
remove_pose_service(named_pose)

Or bash:

rosservice call /arm/remove_named_pose "pose_name: {POSE_TO_REMOVE}"

Getting Saved Named Poses

To see a list of poses are saved, use the /arm/get_named_poses service.

Via Python:

from armer_msgs.srv import GetNamedPoses
import rospy

rospy.init_node('armer_example', disable_signals=True)
get_poses_service = rospy.ServiceProxy('/arm/get_named_poses', GetNamedPoses)

get_poses_service()

Bash:

rosservice call /arm/get_named_poses

Loading Named Poses from Config files

To load configs from a YAML other than the default Armer config, the /arm/add_named_pose_config service can be used.

It can be summoned via Python or Bash.

Python:

from armer_msgs.srv import AddNamedPoseConfig
import rospy

rospy.init_node('armer_example', disable_signals=True)
save_config_service = rospy.ServiceProxy('/arm/add_named_pose_config', AddNamedPoseConfig)

config_path="/home/user/saved_poses.yaml"
save_config_service(config_path)

Bash:

rosservice call /arm/add_named_pose_config "config_path: {PATH_TO_CONFIG.yaml}"

Removing Named Poses from Config files

To remove the config poses, the /arm/remove_named_pose_config service can be called.

Via Python:

from armer_msgs.srv import RemoveNamedPoseConfig
import rospy

rospy.init_node('armer_example', disable_signals=True)
remove_config_service = rospy.ServiceProxy('/arm/remove_named_pose_config', RemoveNamedPoseConfig)

config_path="/home/user/saved_poses.yaml"
remove_config_service(config_path)

Bash:

rosservice call /arm/remove_named_pose_config "config_path: {PATH_TO_CONFIG.yaml}"