Source code for sawyer.arm

# Copyright (c) Facebook, Inc. and its affiliates.

# This source code is licensed under the MIT license found in the
# LICENSE file in the root directory of this source tree.

import intera_interface
import numpy as np
import rospy
from intera_core_msgs.msg import (
from intera_interface import CHECK_VERSION
from pyrobot.core import Arm

[docs]class SawyerArm(Arm): """ This class has functionality to control a Sawyer manipulator. """
[docs] def __init__(self, configs, moveit_planner='ESTkConfigDefault'): """ The constructor for LoCoBotArm class. :param configs: configurations read from config file :param moveit_planner: Planner name for moveit, only used if planning_mode = 'moveit'. :type configs: YACS CfgNode :type moveit_planner: string """ super(SawyerArm, self).__init__(configs=configs, moveit_planner=moveit_planner, use_moveit=True) rs = intera_interface.RobotEnable(CHECK_VERSION) rs.enable() cos_state = self.configs.ARM.ROSTOPIC_COLLISION_STATE self.collision_state_sub = rospy.Subscriber(cos_state, CollisionDetectionState, self._callback_collision, queue_size=1)
[docs] def go_home(self): """ Commands robot to home position """ self.set_joint_positions(np.zeros(self.arm_dof), plan=False)
[docs] def move_to_neutral(self): """ Move the robot to a pre-defined neutral pose """ neutral_pos = [0.002, -1.182, 0.002, 2.177, 0.001, 0.567, 3.316] self.set_joint_positions(neutral_pos, plan=False)
[docs] def get_collision_state(self): """ Return the collision state :return: collision or not :rtype: bool """ return self._collision_state
def _setup_joint_pub(self): self.joint_pub = rospy.Publisher( self.configs.ARM.ROSTOPIC_SET_JOINT, JointCommand, queue_size=1) def _callback_collision(self, data): self._collision_state = data.collision_state def _pub_joint_positions(self, positions): command_msg = JointCommand() command_msg.names = self.arm_joint_names command_msg.position = positions command_msg.mode = JointCommand.POSITION_MODE command_msg.header.stamp = self.joint_pub.publish(command_msg) def _pub_joint_velocities(self, velocities): command_msg = JointCommand() command_msg.names = self.arm_joint_names command_msg.velocity = velocities command_msg.mode = JointCommand.VELOCITY_MODE command_msg.header.stamp = self.joint_pub.publish(command_msg) def _pub_joint_torques(self, torques): command_msg = JointCommand() command_msg.names = self.arm_joint_names command_msg.effort = torques command_msg.mode = JointCommand.TORQUE_MODE command_msg.header.stamp = self.joint_pub.publish(command_msg)