# 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.
from __future__ import print_function
import copy
import importlib
import os
import sys
import threading
import time
from abc import ABCMeta, abstractmethod
import PyKDL as kdl
import moveit_commander
import numpy as np
import rospy
import tf
from geometry_msgs.msg import Twist
from kdl_parser_py.urdf import treeFromParam
from sensor_msgs.msg import JointState
from trac_ik_python import trac_ik
import util as prutil
[docs]class Robot:
"""
This is the main interface class that is composed of
key robot modules (base, arm, gripper, and camera).
This class builds robot specific objects by reading a
configuration and instantiating the necessary robot module objects.
"""
[docs] def __init__(self,
robot_name,
use_arm=True,
use_base=True,
use_camera=True,
use_gripper=True,
arm_config={},
base_config={},
camera_config={},
gripper_config={}):
"""
Constructor for the Robot class
:param robot_name: robot name
:param use_arm: use arm or not
:param use_base: use base or not
:param use_camera: use camera or not
:param use_gripper: use gripper or not
:param arm_config: configurations for arm
:param base_config: configurations for base
:param camera_config: configurations for camera
:param gripper_config: configurations for gripper
:type robot_name: string
:type use_arm: bool
:type use_base: bool
:type use_camera: bool
:type use_gripper: bool
:type arm_config: dict
:type base_config: dict
:type camera_config: dict
:type gripper_config: dict
"""
root_path = os.path.dirname(os.path.realpath(__file__))
cfg_path = os.path.join(root_path, 'cfg')
robot_pool = []
for f in os.listdir(cfg_path):
if f.endswith('_config.py'):
robot_pool.append(f[:-len('_config.py')])
root_node = 'pyrobot.'
self.configs = None
this_robot = None
for srobot in robot_pool:
if srobot in robot_name:
this_robot = srobot
mod = importlib.import_module(root_node + 'cfg.' +
'{:s}_config'.format(srobot))
cfg_func = getattr(mod, 'get_cfg')
if srobot == 'locobot' and 'lite' in robot_name:
self.configs = cfg_func('create')
else:
self.configs = cfg_func()
if self.configs is None:
raise ValueError('Invalid robot name provided, only the following'
' are currently available: {}'.format(robot_pool))
self.configs.freeze()
try:
rospy.init_node('pyrobot', anonymous=True)
except rospy.exceptions.ROSException:
rospy.logwarn('ROS node [pyrobot] has already been initialized')
root_node += this_robot
root_node += '.'
if self.configs.HAS_ARM and use_arm:
mod = importlib.import_module(root_node + 'arm')
arm_class = getattr(mod, self.configs.ARM.CLASS)
self.arm = arm_class(self.configs, **arm_config)
if self.configs.HAS_BASE and use_base:
mod = importlib.import_module(root_node + 'base')
base_class = getattr(mod, self.configs.BASE.CLASS)
self.base = base_class(self.configs, **base_config)
if self.configs.HAS_CAMERA and use_camera:
mod = importlib.import_module(root_node + 'camera')
camera_class = getattr(mod, self.configs.CAMERA.CLASS)
self.camera = camera_class(self.configs, **camera_config)
if self.configs.HAS_GRIPPER and use_gripper and use_arm:
mod = importlib.import_module(root_node + 'gripper')
gripper_class = getattr(mod, self.configs.GRIPPER.CLASS)
self.gripper = gripper_class(self.configs, **gripper_config)
# sleep some time for tf listeners in subclasses
rospy.sleep(2)
[docs]class Base(object):
"""
This is a parent class on which the robot
specific Base classes would be built.
"""
[docs] def __init__(self, configs):
"""
The consturctor for Base class.
:param configs: configurations for base
:type configs: YACS CfgNode
"""
self.configs = configs
self.ctrl_pub = rospy.Publisher(configs.BASE.ROSTOPIC_BASE_COMMAND,
Twist, queue_size=1)
[docs] def stop(self):
"""
Stop the base
"""
msg = Twist()
msg.linear.x = 0
msg.angular.z = 0
self.ctrl_pub.publish(msg)
[docs] def set_vel(self, fwd_speed, turn_speed, exe_time=1):
"""
Set the moving velocity of the base
:param fwd_speed: forward speed
:param turn_speed: turning speed
:param exe_time: execution time
"""
fwd_speed = min(fwd_speed, self.configs.BASE.MAX_ABS_FWD_SPEED)
fwd_speed = max(fwd_speed, -self.configs.BASE.MAX_ABS_FWD_SPEED)
turn_speed = min(turn_speed, self.configs.BASE.MAX_ABS_TURN_SPEED)
turn_speed = max(turn_speed, -self.configs.BASE.MAX_ABS_TURN_SPEED)
msg = Twist()
msg.linear.x = fwd_speed
msg.angular.z = turn_speed
start_time = rospy.get_time()
self.ctrl_pub.publish(msg)
while rospy.get_time() - start_time < exe_time:
self.ctrl_pub.publish(msg)
rospy.sleep(1. / self.configs.BASE.BASE_CONTROL_RATE)
[docs] def go_to_relative(self, xyt_position, use_map, close_loop, smooth):
"""
Moves the robot to the robot to given
goal state relative to its initial pose.
:param xyt_position: The relative goal state of the form (x,y,t)
:param use_map: When set to "True", ensures that controler is
using only free space on the map to move the robot.
:param close_loop: When set to "True", ensures that controler is
operating in open loop by
taking account of odometry.
:param smooth: When set to "True", ensures that the motion
leading to the goal is a smooth one.
:type xyt_position: list
:type use_map: bool
:type close_loop: bool
:type smooth: bool
"""
raise NotImplementedError
[docs] def go_to_absolute(self, xyt_position, use_map, close_loop, smooth):
"""
Moves the robot to the robot to given goal state in the world frame.
:param xyt_position: The goal state of the form (x,y,t)
in the world (map) frame.
:param use_map: When set to "True", ensures that controler is using
only free space on the map to move the robot.
:param close_loop: When set to "True", ensures that controler is
operating in open loop by
taking account of odometry.
:param smooth: When set to "True", ensures that the motion
leading to the goal is a smooth one.
:type xyt_position: list
:type use_map: bool
:type close_loop: bool
:type smooth: bool
"""
raise NotImplementedError
[docs] def track_trajectory(self, states, controls, close_loop):
"""
State trajectory that the robot should track.
:param states: sequence of (x,y,t) states that the robot should track.
:param controls: optionally specify control sequence as well.
:param close_loop: whether to close loop on the
computed control sequence or not.
:type states: list
:type controls: list
:type close_loop: bool
"""
raise NotImplementedError
[docs] def get_state(self, state_type):
"""
Returns the requested base pose in the (x,y, yaw) format.
:param state_type: Requested state type. Ex: Odom, SLAM, etc
:type state_type: string
:return: pose: pose of the form [x, y, yaw]
:rtype: list
"""
raise NotImplementedError
[docs]class Gripper(object):
"""
This is a parent class on which the robot
specific Gripper classes would be built.
"""
__metaclass__ = ABCMeta
[docs] def __init__(self, configs):
"""
Constructor for Gripper parent class.
:param configs: configurations for gripper
:type configs: YACS CfgNode
"""
self.configs = configs
@abstractmethod
def open(self, **kwargs):
pass
@abstractmethod
def close(self, **kwargs):
pass
[docs]class Camera(object):
"""
This is a parent class on which the robot
specific Camera classes would be built.
"""
__metaclass__ = ABCMeta
[docs] def __init__(self, configs):
"""
Constructor for Camera parent class.
:param configs: configurations for camera
:type configs: YACS CfgNode
"""
self.configs = configs
@abstractmethod
def get_rgb(self, **kwargs):
pass
[docs]class Arm(object):
"""
This is a parent class on which the robot
specific Arm classes would be built.
"""
__metaclass__ = ABCMeta
[docs] def __init__(self,
configs,
moveit_planner='ESTkConfigDefault',
analytical_ik=None,
use_moveit=True):
"""
Constructor for Arm parent class.
:param configs: configurations for arm
:param moveit_planner: moveit planner type
:param analytical_ik: customized analytical ik class
if you have one, None otherwise
:param use_moveit: use moveit or not, default is True
:type configs: YACS CfgNode
:type moveit_planner: string
:type analytical_ik: None or an Analytical ik class
:type use_moveit: bool
"""
self.configs = configs
self.moveit_planner = moveit_planner
self.moveit_group = None
self.scene = None
self.use_moveit = use_moveit
self.joint_state_lock = threading.RLock()
self.tf_listener = tf.TransformListener()
if self.use_moveit:
self._init_moveit()
robot_description = self.configs.ARM.ARM_ROBOT_DSP_PARAM_NAME
urdf_string = rospy.get_param(robot_description)
self.num_ik_solver = trac_ik.IK(configs.ARM.ARM_BASE_FRAME,
configs.ARM.EE_FRAME,
urdf_string=urdf_string)
if analytical_ik is not None:
self.ana_ik_solver = analytical_ik(configs.ARM.ARM_BASE_FRAME,
configs.ARM.EE_FRAME)
_, self.urdf_tree = treeFromParam(robot_description)
self.urdf_chain = self.urdf_tree.getChain(configs.ARM.ARM_BASE_FRAME,
configs.ARM.EE_FRAME)
self.arm_joint_names = self._get_kdl_joint_names()
self.arm_link_names = self._get_kdl_link_names()
self.arm_dof = len(self.arm_joint_names)
self.jac_solver = kdl.ChainJntToJacSolver(self.urdf_chain)
self.fk_solver_pos = kdl.ChainFkSolverPos_recursive(self.urdf_chain)
self.fk_solver_vel = kdl.ChainFkSolverVel_recursive(self.urdf_chain)
# Subscribers
self._joint_angles = dict()
self._joint_velocities = dict()
self._joint_efforts = dict()
rospy.Subscriber(configs.ARM.ROSTOPIC_JOINT_STATES, JointState,
self._callback_joint_states)
# Publishers
self.joint_pub = None
self._setup_joint_pub()
[docs] @abstractmethod
def go_home(self):
"""
Reset robot to default home position
"""
pass
@property
def pose_ee(self):
"""
Return the end effector pose w.r.t 'ARM_BASE_FRAME'
:return:
trans: translational vector (shape: :math:`[3, 1]`)
rot_mat: rotational matrix (shape: :math:`[3, 3]`)
quat: rotational matrix in the form
of quaternion (shape: :math:`[4,]`)
:rtype: tuple (trans, rot_mat, quat)
"""
return self.get_ee_pose(base_frame=self.configs.ARM.ARM_BASE_FRAME)
[docs] def get_ee_pose(self, base_frame):
"""
Return the end effector pose with respect to the base_frame
:param base_frame: reference frame
:type base_frame: string
:return:
tuple (trans, rot_mat, quat)
trans: translational vector (shape: :math:`[3, 1]`)
rot_mat: rotational matrix (shape: :math:`[3, 3]`)
quat: rotational matrix in the form
of quaternion (shape: :math:`[4,]`)
:rtype: tuple or ROS PoseStamped
"""
return self.get_transform(base_frame, self.configs.ARM.EE_FRAME)
[docs] def get_joint_angles(self):
"""
Return the joint angles
:return: joint_angles
:rtype: np.ndarray
"""
self.joint_state_lock.acquire()
joint_angles = []
for joint in self.arm_joint_names:
joint_angles.append(self.get_joint_angle(joint))
joint_angles = np.array(joint_angles).flatten()
self.joint_state_lock.release()
return joint_angles
[docs] def get_joint_velocities(self):
"""
Return the joint velocities
:return: joint_vels
:rtype: np.ndarray
"""
self.joint_state_lock.acquire()
joint_vels = []
for joint in self.arm_joint_names:
joint_vels.append(self.get_joint_velocity(joint))
joint_vels = np.array(joint_vels).flatten()
self.joint_state_lock.release()
return joint_vels
[docs] def get_joint_torques(self):
"""
Return the joint torques
:return: joint_torques
:rtype: np.ndarray
"""
self.joint_state_lock.acquire()
joint_torques = []
for joint in self.arm_joint_names:
try:
joint_torques.append(self.get_joint_torque(joint))
except (ValueError, IndexError):
rospy.loginfo('Torque value for joint '
'[%s] not available!' % joint)
joint_torques = np.array(joint_torques).flatten()
self.joint_state_lock.release()
return joint_torques
[docs] def get_joint_angle(self, joint):
"""
Return the joint angle of the 'joint'
:param joint: joint name
:type joint: string
:return: joint angle
:rtype: float
"""
if joint not in self.arm_joint_names:
raise ValueError('%s not in arm joint list!' % joint)
if joint not in self._joint_angles.keys():
raise ValueError('Joint angle for joint $s not available!' % joint)
return self._joint_angles[joint]
[docs] def get_joint_velocity(self, joint):
"""
Return the joint velocity of the 'joint'
:param joint: joint name
:type joint: string
:return: joint velocity
:rtype: float
"""
if joint not in self.arm_joint_names:
raise ValueError('%s not in arm joint list!' % joint)
if joint not in self._joint_velocities.keys():
raise ValueError('Joint velocity for joint'
' $s not available!' % joint)
return self._joint_velocities[joint]
[docs] def get_joint_torque(self, joint):
"""
Return the joint torque of the 'joint'
:param joint: joint name
:type joint: string
:return: joint torque
:rtype: float
"""
if joint not in self.arm_joint_names:
raise ValueError('%s not in arm joint list!' % joint)
if joint not in self._joint_efforts.keys():
raise ValueError('Joint torque for joint $s'
' not available!' % joint)
return self._joint_efforts[joint]
[docs] def set_joint_positions(self, positions, plan=True, wait=True, **kwargs):
"""
Sets the desired joint angles for all arm joints
:param positions: list of length #of joints, angles in radians
:param plan: whether to use moveit to plan a path. Without planning,
there is no collision checking and each joint will
move to its target joint position directly.
:param wait: if True, it will wait until the desired
joint positions are achieved
:type positions: list
:type plan: bool
:type wait: bool
:return: True if successful; False otherwise (timeout, etc.)
:rtype: bool
"""
result = False
if isinstance(positions, np.ndarray):
positions = positions.flatten().tolist()
if plan:
moveit_plan = self._compute_plan(positions)
result = self._execute_plan(moveit_plan, wait=wait)
else:
self._pub_joint_positions(positions)
if wait:
result = self._loop_angle_pub_cmd(self._pub_joint_positions,
positions)
return result
[docs] def set_joint_velocities(self, velocities, **kwargs):
"""
Sets the desired joint velocities for all arm joints
:param velocities: target joint velocities
:type velocities: list
"""
self._pub_joint_velocities(velocities)
[docs] def set_joint_torques(self, torques, **kwargs):
"""
Sets the desired joint torques for all arm joints
:param torques: target joint torques
:type torques: list
"""
self._pub_joint_torques(torques)
[docs] def set_ee_pose(self, position, orientation, 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 orientation: orientation of the end effector
(can be rotation matrix, euler angles (yaw,
pitch, roll), or quaternion)
(shape: :math:`[3, 3]`, :math:`[3,]`
or :math:`[4,]`)
The convention of the Euler angles here
is z-y'-x' (intrinsic rotations),
which is one type of Tait-Bryan angles.
: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 orientation: np.ndarray
:type plan: bool
:type wait: bool
:type numerical: bool
:return: Returns True if command succeeded, False otherwise
:rtype: bool
"""
joint_positions = self.compute_ik(position, orientation,
numerical=numerical)
result = False
if joint_positions is None:
rospy.logerr('No IK solution found; check if target_pose is valid')
else:
result = self.set_joint_positions(joint_positions,
plan=plan, wait=wait)
return result
[docs] def move_ee_xyz(self, displacement, eef_step=0.005,
numerical=True, plan=True, **kwargs):
"""
Keep the current orientation fixed, move the end
effector in {xyz} directions
:param displacement: (delta_x, delta_y, delta_z)
:param eef_step: resolution (m) of the interpolation
on the cartesian path
:param numerical: use numerical inverse kinematics solver or
analytical inverse kinematics solver
:param plan: use moveit the plan a path to move to the
desired pose. If False,
it will do linear interpolation along the path,
and simply use IK solver to find the
sequence of desired joint positions and
then call `set_joint_positions`
:type displacement: np.ndarray
:type eef_step: float
:type numerical: bool
:type plan: bool
:return: whether the movement is successful or not
:rtype: bool
"""
displacement = displacement.reshape(-1, 1)
path_len = np.linalg.norm(displacement)
num_pts = int(np.ceil(path_len / float(eef_step)))
if num_pts <= 1:
num_pts = 2
if (hasattr(self, 'ana_ik_solver') and not numerical) or not plan:
ee_pose = self.get_ee_pose(self.configs.ARM.ARM_BASE_FRAME)
cur_pos, cur_ori, cur_quat = ee_pose
waypoints_sp = np.linspace(0, path_len, num_pts)
waypoints = cur_pos + waypoints_sp / float(path_len) * displacement
way_joint_positions = []
qinit = self.get_joint_angles().tolist()
for i in range(waypoints.shape[1]):
joint_positions = self.compute_ik(waypoints[:, i].flatten(),
cur_quat,
qinit=qinit,
numerical=numerical)
if joint_positions is None:
rospy.logerr('No IK solution found; '
'check if target_pose is valid')
return False
way_joint_positions.append(copy.deepcopy(joint_positions))
qinit = copy.deepcopy(joint_positions)
success = False
for joint_positions in way_joint_positions:
success = self.set_joint_positions(joint_positions,
plan=plan, wait=True)
return success
else:
if not self.use_moveit:
raise ValueError('Using plan=True when moveit is not'
' initialized, did you pass '
'in use_moveit=True?')
# ee_pose = self.get_ee_pose(self.configs.ARM.ARM_BASE_FRAME)
# cur_pos, cur_ori, cur_quat = ee_pose
cur_pose = self.moveit_group.get_current_pose()
cur_pos = np.array([cur_pose.pose.position.x,
cur_pose.pose.position.y,
cur_pose.pose.position.z]).reshape(-1, 1)
cur_quat = np.array([cur_pose.pose.orientation.x,
cur_pose.pose.orientation.y,
cur_pose.pose.orientation.z,
cur_pose.pose.orientation.w])
waypoints_sp = np.linspace(0, path_len, num_pts)
waypoints = cur_pos + waypoints_sp / path_len * displacement
moveit_waypoints = []
wpose = self.moveit_group.get_current_pose().pose
for i in range(waypoints.shape[1]):
wpose.position.x = waypoints[0, i]
wpose.position.y = waypoints[1, i]
wpose.position.z = waypoints[2, i]
wpose.orientation.x = cur_quat[0]
wpose.orientation.y = cur_quat[1]
wpose.orientation.z = cur_quat[2]
wpose.orientation.w = cur_quat[3]
moveit_waypoints.append(copy.deepcopy(wpose))
(plan, fraction) = self.moveit_group.compute_cartesian_path(
moveit_waypoints, # waypoints to follow
eef_step, # eef_step
0.0) # jump_threshold
self._execute_plan(plan, wait=True)
cur_pose = self.moveit_group.get_current_pose()
cur_pos = np.array([cur_pose.pose.position.x,
cur_pose.pose.position.y,
cur_pose.pose.position.z]).reshape(-1, 1)
success = True
diff = cur_pos.flatten() - waypoints[:, -1].flatten()
error = np.linalg.norm(diff)
if error > self.configs.ARM.MAX_EE_ERROR:
rospy.logerr('Move end effector along xyz failed!')
success = False
return success
[docs] def get_jacobian(self, joint_angles):
"""
Return the geometric jacobian on the given joint angles.
Refer to P112 in "Robotics: Modeling, Planning, and Control"
:param joint_angles: joint_angles
:type joint_angles: list or flattened np.ndarray
:return: jacobian
:rtype: np.ndarray
"""
q = kdl.JntArray(self.urdf_chain.getNrOfJoints())
for i in range(q.rows()):
q[i] = joint_angles[i]
jac = kdl.Jacobian(self.urdf_chain.getNrOfJoints())
fg = self.jac_solver.JntToJac(q, jac)
assert fg == 0, 'KDL JntToJac error!'
jac_np = prutil.kdl_array_to_numpy(jac)
return jac_np
[docs] def compute_fk_position(self, joint_positions, des_frame):
"""
Given joint angles, compute the pose of desired_frame with respect
to the base frame (self.configs.ARM.ARM_BASE_FRAME). The desired frame
must be in self.arm_link_names
:param joint_positions: joint angles
:param des_frame: desired frame
:type joint_positions: np.ndarray
:type des_frame: string
:return: translational vector and rotational matrix
:rtype: np.ndarray, np.ndarray
"""
joint_positions = joint_positions.flatten()
assert joint_positions.size == self.arm_dof
kdl_jnt_angles = prutil.joints_to_kdl(joint_positions)
kdl_end_frame = kdl.Frame()
idx = self.arm_link_names.index(des_frame) + 1
fg = self.fk_solver_pos.JntToCart(kdl_jnt_angles,
kdl_end_frame,
idx)
assert fg == 0, 'KDL Pos JntToCart error!'
pose = prutil.kdl_frame_to_numpy(kdl_end_frame)
pos = pose[:3, 3].reshape(-1, 1)
rot = pose[:3, :3]
return pos, rot
[docs] def compute_fk_velocity(self, joint_positions,
joint_velocities, des_frame):
"""
Given joint_positions and joint velocities,
compute the velocities of des_frame with respect
to the base frame
:param joint_positions: joint positions
:param joint_velocities: joint velocities
:param des_frame: end frame
:type joint_positions: np.ndarray
:type joint_velocities: np.ndarray
:type des_frame: string
:return: translational and rotational
velocities (vx, vy, vz, wx, wy, wz)
:rtype: np.ndarray
"""
kdl_end_frame = kdl.FrameVel()
kdl_jnt_angles = prutil.joints_to_kdl(joint_positions)
kdl_jnt_vels = prutil.joints_to_kdl(joint_velocities)
kdl_jnt_qvels = kdl.JntArrayVel(kdl_jnt_angles, kdl_jnt_vels)
idx = self.arm_link_names.index(des_frame) + 1
fg = self.fk_solver_vel.JntToCart(kdl_jnt_qvels,
kdl_end_frame,
idx)
assert fg == 0, 'KDL Vel JntToCart error!'
end_twist = kdl_end_frame.GetTwist()
return np.array([end_twist[0], end_twist[1], end_twist[2],
end_twist[3], end_twist[4], end_twist[5]])
[docs] def compute_ik(self, position, orientation, qinit=None, numerical=True):
"""
Inverse kinematics
:param position: end effector position (shape: :math:`[3,]`)
:param orientation: end effector orientation.
It can be quaternion ([x,y,z,w],
shape: :math:`[4,]`),
euler angles (yaw, pitch, roll
angles (shape: :math:`[3,]`)),
or rotation matrix (shape: :math:`[3, 3]`)
:param qinit: initial joint positions for numerical IK
:param numerical: use numerical IK or analytical IK
:type position: np.ndarray
:type orientation: np.ndarray
:type qinit: list
:type numerical: bool
:return: None or joint positions
:rtype: np.ndarray
"""
position = position.flatten()
if orientation.size == 4:
orientation = orientation.flatten()
ori_x = orientation[0]
ori_y = orientation[1]
ori_z = orientation[2]
ori_w = orientation[3]
elif orientation.size == 3:
quat = prutil.euler_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
elif orientation.size == 9:
quat = prutil.rot_mat_to_quat(orientation)
ori_x = quat[0]
ori_y = quat[1]
ori_z = quat[2]
ori_w = quat[3]
else:
raise TypeError('Orientation must be in one '
'of the following forms:'
'rotation matrix, euler angles, or quaternion')
if qinit is None:
qinit = self.get_joint_angles().tolist()
elif isinstance(qinit, np.ndarray):
qinit = qinit.flatten().tolist()
if numerical:
pos_tol = self.configs.ARM.IK_POSITION_TOLERANCE
ori_tol = self.configs.ARM.IK_ORIENTATION_TOLERANCE
joint_positions = self.num_ik_solver.get_ik(qinit,
position[0],
position[1],
position[2],
ori_x,
ori_y,
ori_z,
ori_w,
pos_tol,
pos_tol,
pos_tol,
ori_tol,
ori_tol,
ori_tol)
else:
if not hasattr(self, 'ana_ik_solver'):
raise TypeError('Analytical solver not provided, '
'please use `numerical=True`'
'to use the numerical method '
'for inverse kinematics')
else:
joint_positions = self.ana_ik_solver.get_ik(qinit,
position[0],
position[1],
position[2],
ori_x,
ori_y,
ori_z,
ori_w)
if joint_positions is None:
return None
return np.array(joint_positions)
def _get_kdl_link_names(self):
num_links = self.urdf_chain.getNrOfSegments()
link_names = []
for i in range(num_links):
link_names.append(self.urdf_chain.getSegment(i).getName())
return copy.deepcopy(link_names)
def _get_kdl_joint_names(self):
num_links = self.urdf_chain.getNrOfSegments()
num_joints = self.urdf_chain.getNrOfJoints()
joint_names = []
for i in range(num_links):
link = self.urdf_chain.getSegment(i)
joint = link.getJoint()
joint_type = joint.getType()
# JointType definition: [RotAxis,RotX,RotY,RotZ,TransAxis,
# TransX,TransY,TransZ,None]
if joint_type > 1:
continue
joint_names.append(joint.getName())
assert num_joints == len(joint_names)
return copy.deepcopy(joint_names)
def _callback_joint_states(self, msg):
"""
ROS subscriber callback for arm joint state (position, velocity)
:param msg: Contains message published in topic
:type msg: sensor_msgs/JointState
"""
self.joint_state_lock.acquire()
for idx, name in enumerate(msg.name):
if name in self.arm_joint_names:
if idx < len(msg.position):
self._joint_angles[name] = msg.position[idx]
if idx < len(msg.velocity):
self._joint_velocities[name] = msg.velocity[idx]
if idx < len(msg.effort):
self._joint_efforts[name] = msg.effort[idx]
self.joint_state_lock.release()
def _pub_joint_positions(self, positions):
joint_state = JointState()
joint_state.position = tuple(positions)
self.joint_pub.publish(joint_state)
def _pub_joint_velocities(self, velocities):
joint_state = JointState()
joint_state.velocity = tuple(velocities)
self.joint_pub.publish(joint_state)
def _pub_joint_torques(self, torques):
joint_state = JointState()
joint_state.effort = tuple(torques)
self.joint_pub.publish(joint_state)
def _init_moveit(self):
"""
Initialize moveit and setup move_group object
"""
moveit_commander.roscpp_initialize(sys.argv)
mg_name = self.configs.ARM.MOVEGROUP_NAME
self.moveit_group = moveit_commander.MoveGroupCommander(mg_name)
self.moveit_group.set_planner_id(self.moveit_planner)
self.scene = moveit_commander.PlanningSceneInterface()
def _compute_plan(self, target_joint):
"""
Computes motion plan to achieve desired target_joint
:param target_joint: list of length #of joints, angles in radians
:type target_joint: np.ndarray
:return: Computed motion plan
:rtype: moveit_msgs.msg.RobotTrajectory
"""
# TODO Check if target_joint is valid
if not self.use_moveit:
raise ValueError('Moveit is not initialized, '
'did you pass in use_moveit=True?')
if isinstance(target_joint, np.ndarray):
target_joint = target_joint.tolist()
self.moveit_group.set_joint_value_target(target_joint)
rospy.loginfo('Moveit Motion Planning...')
return self.moveit_group.plan()
def _execute_plan(self, plan, wait=True):
"""
Executes plan on arm controller
:param plan: motion plan to execute
:param wait: True if blocking call and will return after
target_joint is achieved, False otherwise
:type plan: moveit_msgs.msg.RobotTrajectory
:type wait: bool
:return: True if arm executed plan, False otherwise
:rtype: bool
"""
result = False
if not self.use_moveit:
raise ValueError('Moveit is not initialized, '
'did you pass in use_moveit=True?')
if len(plan.joint_trajectory.points) < 1:
rospy.logwarn('No motion plan found. No execution attempted')
else:
rospy.loginfo('Executing...')
result = self.moveit_group.execute(plan, wait=wait)
return result
def _angle_error_is_small(self, target_joints):
cur_joint_vals = self.get_joint_angles()
joint_diff = cur_joint_vals - np.array(target_joints)
error = np.max(np.abs(joint_diff))
if error <= self.configs.ARM.MAX_JOINT_ERROR:
return joint_diff, error, True
else:
return joint_diff, error, False
def _loop_angle_pub_cmd(self, cmd, joint_vals):
start_time = time.time()
vel_zero_times = 0
# wait 10s in worse case
for i in range(int(10 / self.configs.ARM.WAIT_MIN_TIME)):
cmd(joint_vals)
res = self._angle_error_is_small(joint_vals)
joint_diff, error, eflag = res
if eflag:
success = True
break
vels = self.get_joint_velocities()
es_time = time.time() - start_time
if es_time > 0.5 and np.max(np.abs(vels)) < 0.01:
vel_zero_times += 1
else:
vel_zero_times = 0
if vel_zero_times > 10:
success = False
break
rospy.sleep(self.configs.ARM.WAIT_MIN_TIME)
return success
def _setup_joint_pub(self):
self.joint_pub = rospy.Publisher(
self.configs.ARM.ROSTOPIC_SET_JOINT, JointState, queue_size=1)