# 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 numpy as np
import rospy
from locobot_control.analytic_ik import AnalyticInverseKinematics as AIK
from locobot_control.srv import JointCommand
from pyrobot.core import Arm
from std_msgs.msg import Empty
[docs]class LoCoBotArm(Arm):
"""
This class has functionality to control a robotic arm
in joint and task space (with and
without any motion planning), for position/velocity/torque control, etc.
"""
[docs] def __init__(self,
configs,
control_mode='position',
moveit_planner='ESTkConfigDefault',
use_moveit=True):
"""
The constructor for LoCoBotArm class.
:param configs: configurations read from config file
:param control_mode: Choose between 'position',
'velocity' and 'torque' control
:param moveit_planner: Planner name for moveit,
only used if planning_mode = 'moveit'.
:param use_moveit: use moveit or not, default is True
:type configs: YACS CfgNode
:type control_mode: string
:type moveit_planner: string
:type use_moveit: bool
"""
use_arm = rospy.get_param('use_arm', False)
use_sim = rospy.get_param('use_sim', False)
use_arm = use_arm or use_sim
if not use_arm:
rospy.logwarn('Neither use_arm, nor use_sim, is not set to '
'True when the LoCoBot driver is launched.'
'You may not be able to command the '
'arm correctly using PyRobot!!!')
return
self.CONTROL_MODES = {'position': 0, 'velocity': 1, 'torque': 2}
self.mode_control = self.CONTROL_MODES[control_mode]
super(LoCoBotArm, self).__init__(configs=configs,
moveit_planner=moveit_planner,
analytical_ik=AIK,
use_moveit=use_moveit)
self.joint_stop_pub = rospy.Publisher(
self.configs.ARM.ROSTOPIC_STOP_EXECUTION, Empty, queue_size=1)
# Services
if self.mode_control == self.CONTROL_MODES['position']:
self.joint_cmd_srv = rospy.ServiceProxy(
self.configs.ARM.ROSSERVICE_JOINT_COMMAND, JointCommand)
elif self.mode_control == self.CONTROL_MODES['torque']:
self.torque_cmd_srv = rospy.ServiceProxy(
self.configs.ARM.ROSTOPIC_TORQUE_COMMAND, JointCommand)
[docs] def set_joint_velocities(self, velocities, **kwargs):
raise NotImplementedError('Velocity control for '
'locobot not supported yet!')
[docs] def set_joint_torque(self, joint_name, value):
"""
:param joint_name: joint name (['joint_1',
'joint_2', 'joint_3', 'joint_4''])
:param value: torque value in Nm
:type joint_name: string
:type value: float
:return: sucessful or not
:rtype: bool
"""
joint_id_dict = {
'joint_1': 1,
'joint_2': 2,
'joint_3': 3,
'joint_4': 4}
if joint_name in joint_id_dict:
return self.torque_cmd_srv(
'newt', joint_id_dict[joint_name], value)
else:
rospy.logerr(
"{} joint name provided, it should be one of this {}".format(
joint_name, sorted(joint_id_dict.keys())))
return False
[docs] def set_ee_pose_pitch_roll(self, position, pitch, roll=None, plan=True,
wait=True, numerical=True, **kwargs):
"""
Commands robot arm to desired end-effector pose
(w.r.t. 'ARM_BASE_FRAME').
Computes IK solution in joint space and calls set_joint_positions.
Will wait for command to complete if wait is set to True.
:param position: position of the end effector (shape: :math:`[3,]`)
:param pitch: pitch angle
:param roll: roll angle
:param plan: use moveit the plan a path to move to the desired pose
:param wait: wait until the desired pose is achieved
:param numerical: use numerical inverse kinematics solver or
analytical inverse kinematics solver
:type position: np.ndarray
:type pitch: float
:type roll: float
:type plan: bool
:type wait: bool
:type numerical: bool
:return result: Returns True if command succeeded, False otherwise
:rtype: bool
"""
position = np.array(position).flatten()
base_offset, _, _ = self.get_transform(self.configs.ARM.ARM_BASE_FRAME,
'arm_base_link')
yaw = np.arctan2(position[1] - base_offset[1],
position[0] - base_offset[0])
if roll is None:
# read the current roll angle
roll = -self.get_joint_angle('joint_5')
euler = np.array([yaw, pitch, roll])
return self.set_ee_pose(position=position, orientation=euler,
plan=plan, wait=wait,
numerical=numerical, **kwargs)
[docs] def set_joint_torques(self, torques, **kwargs):
"""
Sets the desired joint torques for all arm joints.
:param torques: target joint torques, list of len 4 populated
with torque to be applied on first 4 joints
of arm in Nm
:type torques: list
"""
if len(torques) == 4:
joint_id_list = ['joint_1', 'joint_2', 'joint_3', 'joint_4']
for index, value in enumerate(torques):
self.set_joint_torque(joint_id_list[index], value)
else:
rospy.logerr("It is expecting input of type list of length 4")
[docs] def go_home(self, plan=False):
"""
Commands robot to home position
:param plan: use moveit to plan the path or not
:type plan: bool
"""
return self.set_joint_positions(np.zeros(self.arm_dof), plan=plan)