def test_MoveHand(self):
"""Test MoveHandAction."""
self.robot.arm = self.robot.GetManipulator('arm')
self.robot.hand = self.robot.arm.GetEndEffector()
prpy.bind_subclass(self.robot.hand,
DummyHand,
manipulator=self.robot.arm)
action = MoveHandAction(self.robot.hand, [0.7],
self.robot.hand.GetIndices()[:1])
return self._action_helper(action)
def initialize(env_path=None, attach_viewer=False, **kw_args):
from adarobot import ADARobot
from util import AdaPyException, find_adapy_resource
prpy.logger.initialize_logging()
# Create the environment.
env = openravepy.Environment()
if env_path is not None:
if not env.Load(env_path):
raise IOError(
'Unable to load environment "{:s}".'.format(env_path))
# Use or_urdf to load ADA from URDF and SRDF.
with env:
or_urdf = openravepy.RaveCreateModule(env, 'urdf')
ada_name = or_urdf.SendCommand('load {:s} {:s}'.format(
URDF_PATH, SRDF_PATH))
robot = env.GetRobot(ada_name)
if robot is None:
raise AdaPyException('Failed loading ADA with or_urdf.')
# Bind AdaPy-specific functionality on the robot.
prpy.bind_subclass(robot, ADARobot, **kw_args)
# Start by attempting to load or_rviz.
if attach_viewer == True:
attach_viewer = 'rviz'
env.SetViewer(attach_viewer)
# Fall back on qtcoin if loading or_rviz failed
if env.GetViewer() is None:
logger.warning('Loading rviz failed. Falling back on qt_coin.')
attach_viewer = 'qtcoin'
if attach_viewer and env.GetViewer() is None:
#Using qtcoin as or_rviz gives seg fault
attach_viewer = 'qtcoin'
env.SetViewer(attach_viewer)
if env.GetViewer() is None:
raise AdaPyException(
'Failed creating viewer of type "{:s}".'.format(attach_viewer))
# Remove the ROS logging handler again. It might have been added when we
# loaded or_rviz.
prpy.logger.remove_ros_logger()
import adapy.action # register actions
import adapy.tsr # register TSR libraries
robot.actions = prpy.action.ActionLibrary()
return env, robot
def __init__(self):
Robot.__init__(self, robot_name='archie')
# Set up ikfast for the robot. May want to add this to a
# Manipulator class init method instead.
self.ikmodel = (
openravepy.databases.inversekinematics.InverseKinematicsModel(
self, iktype=openravepy.IkParameterization.Type.Transform6D))
if not self.ikmodel.load():
self.ikmodel.autogenerate()
self.arm = self.GetManipulator('j2s7s300')
bind_subclass(self.arm, ArchieManipulator)
self.planner = SnapPlanner()
self.arm.sim_controller = self.AttachController(
name=self.arm.GetName(),
args='IdealController',
dof_indices=self.arm.GetArmIndices(),
affine_dofs=0,
simulated=True)
self.SetActiveDOFs(self.arm.GetIndices())
def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
left_hand_sim, right_hand_sim, left_ft_sim,
head_sim, talker_sim, segway_sim, perception_sim):
from prpy.util import FindCatkinResource
Robot.__init__(self, robot_name='herb')
# Controller setup
self.controller_manager = None
self.controllers_always_on = []
self.full_controller_sim = (left_arm_sim and right_arm_sim and
left_ft_sim and right_ft_sim and
left_hand_sim and right_hand_sim and
head_sim)
if not self.full_controller_sim:
# any non-simulation requires ros and the ros_control stack
import rospy
from ros_control_client_py import (ControllerManagerClient,
JointStateClient)
if not rospy.core.is_initialized():
raise RuntimeError('rospy not initialized. '
'Must call rospy.init_node()')
# update openrave state from /joint_states
self._jointstate_client = JointStateClient(
self, topic_name='/joint_states')
self.controller_manager = ControllerManagerClient()
self.controllers_always_on.append('joint_state_controller')
# Convenience attributes for accessing self components.
self.left_arm = self.GetManipulator('left')
self.right_arm = self.GetManipulator('right')
self.head = self.GetManipulator('head')
self.left_arm.hand = self.left_arm.GetEndEffector()
self.right_arm.hand = self.right_arm.GetEndEffector()
self.left_hand = self.left_arm.hand
self.right_hand = self.right_arm.hand
self.manipulators = [self.left_arm, self.right_arm, self.head]
# Dynamically switch to self-specific subclasses.
prpy.bind_subclass(self.left_arm, WAM, sim=left_arm_sim, namespace='/left')
prpy.bind_subclass(self.right_arm, WAM, sim=right_arm_sim, namespace='/right')
prpy.bind_subclass(self.head, HERBPantilt, sim=head_sim, owd_namespace='/head/owd')
prpy.bind_subclass(self.left_arm.hand, BarrettHand, sim=left_hand_sim, manipulator=self.left_arm,
bhd_namespace='/left', ft_sim=left_ft_sim)
prpy.bind_subclass(self.right_arm.hand, BarrettHand, sim=right_hand_sim, manipulator=self.right_arm,
bhd_namespace='/right', ft_sim=right_ft_sim)
self.base = HerbBase(sim=segway_sim, robot=self)
# Set HERB's acceleration limits. These are not specified in URDF.
accel_limits = self.GetDOFAccelerationLimits()
accel_limits[self.head.GetArmIndices()] = [ 2. ] * self.head.GetArmDOF()
accel_limits[self.left_arm.GetArmIndices()] = [ 2. ] * self.left_arm.GetArmDOF()
accel_limits[self.right_arm.GetArmIndices()] = [ 2. ] * self.right_arm.GetArmDOF()
self.SetDOFAccelerationLimits(accel_limits)
# Determine always-on controllers
# hand controllers
if not left_hand_sim:
self.controllers_always_on.append('left_hand_controller')
if not right_hand_sim:
self.controllers_always_on.append('right_hand_controller')
# force/torque controllers
if not left_ft_sim or not right_ft_sim:
self.controllers_always_on.append('force_torque_sensor_controller')
if not left_ft_sim:
self.controllers_always_on.append('left_tare_controller')
if not right_ft_sim:
self.controllers_always_on.append('right_tare_controller')
# Set default manipulator controllers in sim only
# NOTE: head is ignored until TODO new Schunk head integrated
if left_arm_sim:
self.left_arm.sim_controller = self.AttachController(name=self.left_arm.GetName(),
args='IdealController',
dof_indices=self.left_arm.GetArmIndices(),
affine_dofs=0,
simulated=True)
if right_arm_sim:
self.right_arm.sim_controller = self.AttachController(name=self.right_arm.GetName(),
args='IdealController',
dof_indices=self.right_arm.GetArmIndices(),
affine_dofs=0,
simulated=True)
# load and activate initial controllers
if self.controller_manager is not None:
self.controller_manager.request(
self.controllers_always_on).switch()
# Support for named configurations.
import os.path
self.configurations.add_group('left_arm', self.left_arm.GetArmIndices())
self.configurations.add_group('right_arm', self.right_arm.GetArmIndices())
self.configurations.add_group('head', self.head.GetArmIndices())
self.configurations.add_group('left_hand', self.left_hand.GetIndices())
self.configurations.add_group('right_hand', self.right_hand.GetIndices())
configurations_path = FindCatkinResource('herbpy', 'config/configurations.yaml')
try:
self.configurations.load_yaml(configurations_path)
except IOError as e:
raise ValueError('Failed laoding named configurations from "{:s}".'.format(
configurations_path))
# Hand configurations
from prpy.named_config import ConfigurationLibrary
for hand in [ self.left_hand, self.right_hand ]:
hand.configurations = ConfigurationLibrary()
hand.configurations.add_group('hand', hand.GetIndices())
if isinstance(hand, BarrettHand):
hand_configs_path = FindCatkinResource('herbpy', 'config/barrett_preshapes.yaml')
try:
hand.configurations.load_yaml(hand_configs_path)
except IOError as e:
raise ValueError('Failed loading named hand configurations from "{:s}".'.format(
hand_configs_path))
else:
logger.warning('Unrecognized hand class. Not loading named configurations.')
# Initialize a default planning pipeline.
from prpy.planning import (
FirstSupported,
MethodMask,
Ranked,
Sequence,
)
from prpy.planning import (
CBiRRTPlanner,
CHOMPPlanner,
GreedyIKPlanner,
IKPlanner,
NamedPlanner,
SBPLPlanner,
SnapPlanner,
TSRPlanner,
VectorFieldPlanner
)
# TODO: These should be meta-planners.
self.named_planner = NamedPlanner()
self.ik_planner = IKPlanner()
# Special-purpose planners.
self.snap_planner = SnapPlanner()
self.vectorfield_planner = VectorFieldPlanner()
self.greedyik_planner = GreedyIKPlanner()
# General-purpose planners.
self.cbirrt_planner = CBiRRTPlanner()
# Trajectory optimizer.
try:
from or_trajopt import TrajoptPlanner
self.trajopt_planner = TrajoptPlanner()
except ImportError:
self.trajopt_planner = None
logger.warning('Failed creating TrajoptPlanner. Is the or_trajopt'
' package in your workspace and built?')
try:
self.chomp_planner = CHOMPPlanner()
except UnsupportedPlanningError:
self.chomp_planner = None
logger.warning('Failed loading the CHOMP module. Is the or_cdchomp'
' package in your workspace and built?')
if not (self.trajopt_planner or self.chomp_planner):
raise PrPyException('Unable to load both CHOMP and TrajOpt. At'
' least one of these packages is required.')
actual_planner = Sequence(
# First, try the straight-line trajectory.
self.snap_planner,
# Then, try a few simple (and fast!) heuristics.
self.vectorfield_planner,
#self.greedyik_planner,
# Next, try a trajectory optimizer.
self.trajopt_planner or self.chomp_planner
)
self.planner = FirstSupported(
Sequence(actual_planner,
TSRPlanner(delegate_planner=actual_planner),
self.cbirrt_planner),
# Special purpose meta-planner.
NamedPlanner(delegate_planner=actual_planner),
)
from prpy.planning.retimer import HauserParabolicSmoother
self.smoother = HauserParabolicSmoother(do_blend=True, do_shortcut=True)
self.retimer = HauserParabolicSmoother(do_blend=True, do_shortcut=False,
blend_iterations=5, blend_radius=0.4)
self.simplifier = None
# Base planning
from prpy.util import FindCatkinResource
planner_parameters_path = FindCatkinResource('herbpy', 'config/base_planner_parameters.yaml')
self.sbpl_planner = SBPLPlanner()
try:
with open(planner_parameters_path, 'rb') as config_file:
import yaml
params_yaml = yaml.load(config_file)
self.sbpl_planner.SetPlannerParameters(params_yaml)
except IOError as e:
raise ValueError('Failed loading base planner parameters from "{:s}".'.format(
planner_parameters_path))
self.base_planner = self.sbpl_planner
# Create action library
from prpy.action import ActionLibrary
self.actions = ActionLibrary()
# Register default actions and TSRs
import herbpy.action
import herbpy.tsr
# Setting necessary sim flags
self.talker_simulated = talker_sim
self.segway_sim = segway_sim
# Set up perception
self.detector=None
if perception_sim:
from prpy.perception import SimulatedPerceptionModule
self.detector = SimulatedPerceptionModule()
else:
from prpy.perception import ApriltagsModule
try:
kinbody_path = prpy.util.FindCatkinResource('pr_ordata',
'data/objects')
marker_data_path = prpy.util.FindCatkinResource('pr_ordata',
'data/objects/tag_data.json')
self.detector = ApriltagsModule(marker_topic='/apriltags_kinect2/marker_array',
marker_data_path=marker_data_path,
kinbody_path=kinbody_path,
detection_frame='head/kinect2_rgb_optical_frame',
destination_frame='herb_base',
reference_link=self.GetLink('/herb_base'))
except IOError as e:
logger.warning('Failed to find required resource path. ' \
'pr_ordata package cannot be found. ' \
'Perception detector will not be loaded.' \
'\n{}'.format(e))
if not self.talker_simulated:
# Initialize herbpy ROS Node
import rospy
if not rospy.core.is_initialized():
raise RuntimeError('rospy not initialized. '
'Must call rospy.init_node()')
import talker.msg
from actionlib import SimpleActionClient
self._say_action_client = SimpleActionClient('say', talker.msg.SayAction)
def __init__(self, arm_sim,
hand_sim,
ft_sim,
segway_sim,
):
from prpy.util import FindCatkinResource
Robot.__init__(self, robot_name='BarrettWAM')
# Convenience attributes for accessing self components.
self.arm = self.GetManipulator('arm')
self.arm.hand = self.arm.GetEndEffector()
self.hand = self.arm.hand # 'handbase'
self.manipulators = [ self.arm ]
# Dynamically switch to self-specific subclasses.
prpy.bind_subclass(self.arm, WAM, sim=arm_sim, owd_namespace='/arm/owd')
# namespace must be set, even for sim
prpy.bind_subclass(self.arm.hand, BarrettHand, sim=hand_sim, manipulator=self.arm,
owd_namespace='/arm/owd', bhd_namespace='/arm/bhd', ft_sim=ft_sim)
self.base = WAMBase(sim=segway_sim, robot=self)
#self.base = WAMBase(sim=base_sim, robot=self)
# Set WAM acceleration limits. These are not specified in URDF.
accel_limits = self.GetDOFAccelerationLimits()
accel_limits[self.arm.GetArmIndices()] = [ 2. ] * self.arm.GetArmDOF()
self.SetDOFAccelerationLimits(accel_limits)
# Support for named configurations.
import os.path
self.configurations.add_group('arm', self.arm.GetArmIndices())
self.configurations.add_group('hand', self.hand.GetIndices())
configurations_path = FindCatkinResource('wampy', 'config/configurations.yaml')
try:
self.configurations.load_yaml(configurations_path)
except IOError as e:
raise ValueError('Failed laoding named configurations from "{:s}".'.format(
configurations_path))
# Initialize a default planning pipeline.
from prpy.planning import (
FirstSupported,
MethodMask,
Ranked,
Sequence,
)
from prpy.planning import (
CBiRRTPlanner,
CHOMPPlanner,
GreedyIKPlanner,
IKPlanner,
NamedPlanner,
SBPLPlanner,
SnapPlanner,
TSRPlanner,
VectorFieldPlanner
)
# TODO: These should be meta-planners.
self.named_planner = NamedPlanner()
self.ik_planner = IKPlanner()
# Special-purpose planners.
self.snap_planner = SnapPlanner()
self.vectorfield_planner = VectorFieldPlanner()
self.greedyik_planner = GreedyIKPlanner()
# General-purpose planners.
self.cbirrt_planner = CBiRRTPlanner()
# Trajectory optimizer.
try:
from or_trajopt import TrajoptPlanner
self.trajopt_planner = TrajoptPlanner()
except ImportError:
self.trajopt_planner = None
logger.warning('Failed creating TrajoptPlanner. Is the or_trajopt'
' package in your workspace and built?')
try:
self.chomp_planner = CHOMPPlanner()
except UnsupportedPlanningError:
self.chomp_planner = None
logger.warning('Failed loading the CHOMP module. Is the or_cdchomp'
' package in your workspace and built?')
if not (self.trajopt_planner or self.chomp_planner):
raise PrPyException('Unable to load both CHOMP and TrajOpt. At'
' least one of these packages is required.')
actual_planner = Sequence(
# First, try the straight-line trajectory.
self.snap_planner,
# Then, try a few simple (and fast!) heuristics.
self.vectorfield_planner,
#self.greedyik_planner,
# Next, try a trajectory optimizer.
self.trajopt_planner or self.chomp_planner
)
self.planner = FirstSupported(
Sequence(actual_planner,
TSRPlanner(delegate_planner=actual_planner),
self.cbirrt_planner),
# Special purpose meta-planner.
NamedPlanner(delegate_planner=actual_planner),
)
from prpy.planning.retimer import HauserParabolicSmoother
self.smoother = HauserParabolicSmoother(do_blend=True, do_shortcut=True)
self.retimer = HauserParabolicSmoother(do_blend=True, do_shortcut=False,
blend_iterations=5, blend_radius=0.4)
self.simplifier = None
# Base planning
from prpy.util import FindCatkinResource
planner_parameters_path = FindCatkinResource('wampy', 'config/base_planner_parameters.yaml')
self.sbpl_planner = SBPLPlanner()
try:
with open(planner_parameters_path, 'rb') as config_file:
import yaml
params_yaml = yaml.load(config_file)
self.sbpl_planner.SetPlannerParameters(params_yaml)
except IOError as e:
raise ValueError('Failed loading base planner parameters from "{:s}".'.format(
planner_parameters_path))
self.base_planner = self.sbpl_planner
# Create action library
from prpy.action import ActionLibrary
self.actions = ActionLibrary()
# Register default actions and TSRs
import wampy.action
import wampy.tsr
# Setting necessary sim flags
self.segway_sim = segway_sim
def initialize(robot_xml=None,
env_path=None,
viewer='rviz',
sim=True,
**kw_args):
prpy.logger.initialize_logging()
#Hide TrajOpt logging.
os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN')
#Load plugins
prpy.dependency_manager.export()
RaveInitialize(True)
#Create the environment
env = Environment()
if env_path is not None:
if not env.Load(env_path):
raise ValueError(
'Unable to load environment from path {:s}'.format(env_path))
#Load the URDF file into OpenRave.
urdf_module = RaveCreateModule(env, 'urdf')
if urdf_module is None:
logger.error('Unable to load or_urdf module. Do you have or_urdf'
'built and installed in one of your Catkin workspaces?')
raise ValueError('Unable to load or_urdf plugin.')
urdf_uri = 'package://fetchpy/robot_description/fetch.gazebo.urdf'
srdf_uri = 'package://fetchpy/robot_description/fetch.srdf'
args = 'Load {:s} {:s}'.format(urdf_uri, srdf_uri)
fetch_name = urdf_module.SendCommand(args)
if fetch_name is None:
raise ValueError('Failed loading FETCH model using or_urdf.')
robot = env.GetRobot(fetch_name)
if robot is None:
raise ValueError(
'Unable to find robot with name "{:s}".'.format(fetch_name))
#Default to FCL.
collision_checker = RaveCreateCollisionChecker(env, 'fcl')
if collision_checker is not None:
env.SetCollisionChecker(collision_checker)
else:
collision_checker = env.GetCollisionChecker()
logger.warning(
'Failed creating "fcl", defaulting to the default OpenRAVE'
' collision checker. Did you install or_fcl?')
#Enable Baking if it is supported
try:
result = collision_checker.SendCommand('BakeGetType')
is_baking_suported = (result is not None)
except openrave_exception:
is_baking_suported = False
if is_baking_suported:
robot_checker_factory = BakedRobotCollisionCheckerFactory()
else:
robot_checker_factory = SimpleRobotCollisionCheckerFactory()
logger.warning(
'Collision checker does not support baking. Defaulting to'
' the slower SimpleRobotCollisionCheckerFactory.')
#Default arguments
keys = [
'arm_sim', 'arm_torso_sim', 'gripper_sim', 'head_sim', 'torso_sim',
'base_sim', 'talker_sim', 'perception_sim', 'whole_body_sim'
]
for key in keys:
if key not in kw_args:
kw_args[key] = sim
prpy.bind_subclass(robot,
FETCHRobot,
robot_checker_factory=robot_checker_factory,
**kw_args)
if sim:
dof_indices, dof_values = robot.configurations.get_configuration(
'straight')
robot.SetDOFValues(dof_values, dof_indices)
#Start by attempting to load or_rviz.
viewers = ['qtcoin', 'rviz']
if viewer is None:
viewer = 'rviz'
if viewer not in viewers:
raise Exception(
'Failed creating viewer of type "{0:s}".'.format(viewer))
if viewer == 'rviz':
env.SetViewer(viewer)
if env.GetViewer() is None:
logger.warning(
'Loading the RViz viewer failed. Do you have or_interactive'
' marker installed? Falling back on qtcoin.')
viewer == 'qtcoin'
if viewer == 'qtcoin':
env.SetViewer(viewer)
if viewer and env.GetViewer() is None:
env.SetViewer(viewer)
if env.GetViewer() is None:
raise Exception(
'Failed creating viewer of type "{0:s}".'.format(viewer))
# if attach_viewer == 'True':
# attach_viewer = 'qtcoin'
# env.SetViewer(attach_viewer)
# # Fall back on qtcoin if loading or_rviz failed
# if env.GetViewer() is None:
# logger.warning('Loading the RViz viewer failed. Do you have or_interactive'
# ' marker installed? Falling back on qtcoin.')
# attach_viewer = 'rviz'
# if attach_viewer and env.GetViewer() is None:
# env.SetViewer(attach_viewer)
# if env.GetViewer() is None:
# raise Exception('Failed creating viewer of type "{0:s}".'.format(attach_viewer))
#Remove the ROS logging handler again. It might have been added when we
# loaded or_rviz.
prpy.logger.remove_ros_logger()
return env, robot
def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
left_hand_sim, right_hand_sim, left_ft_sim, head_sim,
talker_sim, segway_sim, perception_sim,
robot_checker_factory):
Robot.__init__(self, robot_name='herb')
self.robot_checker_factory = robot_checker_factory
# Controller setup
self.controller_manager = None
self.controllers_always_on = []
self.full_controller_sim = (left_arm_sim and right_arm_sim
and left_ft_sim and right_ft_sim
and left_hand_sim and right_hand_sim
and head_sim)
if not self.full_controller_sim:
# any non-simulation requires ros and the ros_control stack
import rospy
from ros_control_client_py import (
ControllerManagerClient,
JointStateClient,
)
if not rospy.core.is_initialized():
raise RuntimeError('rospy not initialized. '
'Must call rospy.init_node()')
# update openrave state from /joint_states
self._jointstate_client = JointStateClient(
self, topic_name='/joint_states')
self.controller_manager = ControllerManagerClient()
self.controllers_always_on.append('joint_state_controller')
# Convenience attributes for accessing self components.
self.left_arm = self.GetManipulator('left')
self.right_arm = self.GetManipulator('right')
self.head = self.GetManipulator('head')
self.left_arm.hand = self.left_arm.GetEndEffector()
self.right_arm.hand = self.right_arm.GetEndEffector()
self.left_hand = self.left_arm.hand
self.right_hand = self.right_arm.hand
self.manipulators = [self.left_arm, self.right_arm, self.head]
# Dynamically switch to self-specific subclasses.
prpy.bind_subclass(self.left_arm,
WAM,
sim=left_arm_sim,
namespace='/left')
prpy.bind_subclass(self.right_arm,
WAM,
sim=right_arm_sim,
namespace='/right')
prpy.bind_subclass(self.head,
HERBPantilt,
sim=head_sim,
owd_namespace='/head/owd')
prpy.bind_subclass(self.left_arm.hand,
BarrettHand,
sim=left_hand_sim,
manipulator=self.left_arm,
bhd_namespace='/left',
ft_sim=left_ft_sim)
prpy.bind_subclass(self.right_arm.hand,
BarrettHand,
sim=right_hand_sim,
manipulator=self.right_arm,
bhd_namespace='/right',
ft_sim=right_ft_sim)
self.base = HerbBase(sim=segway_sim, robot=self)
# Set HERB's acceleration limits. These are not specified in URDF.
accel_limits = self.GetDOFAccelerationLimits()
accel_limits[self.head.GetArmIndices()] = [2.] * self.head.GetArmDOF()
accel_limits[
self.left_arm.GetArmIndices()] = [2.] * self.left_arm.GetArmDOF()
accel_limits[self.right_arm.GetArmIndices(
)] = [2.] * self.right_arm.GetArmDOF()
self.SetDOFAccelerationLimits(accel_limits)
# Determine always-on controllers
# hand controllers
if not left_hand_sim:
self.controllers_always_on.append('left_hand_controller')
if not right_hand_sim:
self.controllers_always_on.append('right_hand_controller')
# force/torque controllers
if not left_ft_sim or not right_ft_sim:
self.controllers_always_on.append('force_torque_sensor_controller')
if not left_ft_sim:
self.controllers_always_on.append('left_tare_controller')
if not right_ft_sim:
self.controllers_always_on.append('right_tare_controller')
# Set default manipulator controllers in sim only
# NOTE: head is ignored until TODO new Schunk head integrated
if left_arm_sim:
self.left_arm.sim_controller = self.AttachController(
name=self.left_arm.GetName(),
args='IdealController',
dof_indices=self.left_arm.GetArmIndices(),
affine_dofs=0,
simulated=True)
if right_arm_sim:
self.right_arm.sim_controller = self.AttachController(
name=self.right_arm.GetName(),
args='IdealController',
dof_indices=self.right_arm.GetArmIndices(),
affine_dofs=0,
simulated=True)
# load and activate initial controllers
if self.controller_manager is not None:
self.controller_manager.request(
self.controllers_always_on).switch()
# Support for named configurations.
import os.path
self.configurations.add_group('left_arm',
self.left_arm.GetArmIndices())
self.configurations.add_group('right_arm',
self.right_arm.GetArmIndices())
self.configurations.add_group('head', self.head.GetArmIndices())
self.configurations.add_group('left_hand', self.left_hand.GetIndices())
self.configurations.add_group('right_hand',
self.right_hand.GetIndices())
configurations_path = FindCatkinResource('herbpy',
'config/configurations.yaml')
try:
self.configurations.load_yaml(configurations_path)
except IOError as e:
raise ValueError(
'Failed laoding named configurations from "{:s}".'.format(
configurations_path))
# Hand configurations
for hand in [self.left_hand, self.right_hand]:
hand.configurations = ConfigurationLibrary()
hand.configurations.add_group('hand', hand.GetIndices())
if isinstance(hand, BarrettHand):
hand_configs_path = FindCatkinResource(
'herbpy', 'config/barrett_preshapes.yaml')
try:
hand.configurations.load_yaml(hand_configs_path)
except IOError as e:
raise ValueError(
'Failed loading named hand configurations from "{:s}".'
.format(hand_configs_path))
else:
logger.warning(
'Unrecognized hand class. Not loading named configurations.'
)
# Planner.
snap_planner = SnapPlanner(
robot_checker_factory=self.robot_checker_factory)
vectorfield_planner = VectorFieldPlanner(
robot_checker_factory=self.robot_checker_factory)
trajopt_planner = TrajoptPlanner(
robot_checker_factory=self.robot_checker_factory)
rrt_planner = OMPLPlanner(
'RRTConnect', robot_checker_factory=self.robot_checker_factory)
cbirrt_planner = CBiRRTPlanner(
timelimit=1., robot_checker_factory=self.robot_checker_factory)
actual_planner = Sequence(
snap_planner,
vectorfield_planner,
trajopt_planner,
TSRPlanner(delegate_planner=Sequence(snap_planner,
trajopt_planner),
robot_checker_factory=self.robot_checker_factory),
FirstSupported(rrt_planner, cbirrt_planner),
)
self.planner = FirstSupported(
actual_planner,
NamedPlanner(delegate_planner=actual_planner),
)
# Post-processor.
self.smoother = HauserParabolicSmoother(do_blend=True,
blend_iterations=1,
blend_radius=0.4,
do_shortcut=True,
timelimit=0.6)
self.retimer = HauserParabolicSmoother(do_blend=True,
blend_iterations=1,
blend_radius=0.4,
do_shortcut=False)
self.simplifier = None
# Base planning
planner_parameters_path = FindCatkinResource(
'herbpy', 'config/base_planner_parameters.yaml')
self.sbpl_planner = SBPLPlanner()
try:
with open(planner_parameters_path, 'rb') as config_file:
params_yaml = yaml.load(config_file)
self.sbpl_planner.SetPlannerParameters(params_yaml)
except IOError as e:
raise ValueError(
'Failed loading base planner parameters from "{:s}".'.format(
planner_parameters_path))
self.base_planner = self.sbpl_planner
# Create action library
self.actions = ActionLibrary()
# Register default actions and TSRs
import herbpy.action
import herbpy.tsr
# Setting necessary sim flags
self.talker_simulated = talker_sim
self.segway_sim = segway_sim
# Set up perception
self.detector = None
if perception_sim:
from prpy.perception import SimulatedPerceptionModule
self.detector = SimulatedPerceptionModule()
else:
from prpy.perception import ApriltagsModule
try:
kinbody_path = FindCatkinResource('pr_ordata', 'data/objects')
marker_data_path = FindCatkinResource(
'pr_ordata', 'data/objects/tag_data.json')
self.detector = ApriltagsModule(
marker_topic='/apriltags_kinect2/marker_array',
marker_data_path=marker_data_path,
kinbody_path=kinbody_path,
detection_frame='head/kinect2_rgb_optical_frame',
destination_frame='herb_base',
reference_link=self.GetLink('/herb_base'))
except IOError as e:
logger.warning('Failed to find required resource path. ' \
'pr_ordata package cannot be found. ' \
'Perception detector will not be loaded.' \
'\n{}'.format(e))
if not self.talker_simulated:
# Initialize herbpy ROS Node
import rospy
if not rospy.core.is_initialized():
raise RuntimeError('rospy not initialized. '
'Must call rospy.init_node()')
import talker.msg
from actionlib import SimpleActionClient
self._say_action_client = SimpleActionClient(
'say', talker.msg.SayAction)
def initialize(robot_xml=None, env_path=None, attach_viewer=False,
sim=True, **kw_args):
import prpy, os
prpy.logger.initialize_logging()
# Hide TrajOpt logging.
os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN')
# Load plugins.
if prpy.dependency_manager.is_catkin():
prpy.dependency_manager.export()
else:
prpy.dependency_manager.export(PACKAGE)
openravepy.RaveInitialize(True)
# Create the environment.
env = openravepy.Environment()
if env_path is not None:
if not env.Load(env_path):
raise Exception('Unable to load environment frompath %s' % env_path)
if prpy.dependency_manager.is_catkin():
# Find the HERB URDF and SRDF files.
from catkin.find_in_workspaces import find_in_workspaces
share_directories = find_in_workspaces(search_dirs=['share'],
project='herb_description')
if not share_directories:
logger.error('Unable to find the HERB model. Do you have the'
' package herb_description installed?')
raise ValueError('Unable to find HERB model.')
found_models = False
for share_directory in share_directories:
urdf_path = os.path.join(share_directories[0], 'robots', 'herb.urdf')
srdf_path = os.path.join(share_directories[0], 'robots', 'herb.srdf')
if os.path.exists(urdf_path) and os.path.exists(srdf_path):
found_models = True
break
if not found_models:
logger.error('Missing URDF file and/or SRDF file for HERB.'
' Is the herb_description package properly installed?')
raise ValueError('Unable to find HERB URDF and SRDF files.')
# Load the URDF file into OpenRAVE.
urdf_module = openravepy.RaveCreateModule(env, 'urdf')
if urdf_module is None:
logger.error('Unable to load or_urdf module. Do you have or_urdf'
' built and installed in one of your Catkin workspaces?')
raise ValueError('Unable to load or_urdf plugin.')
args = 'Load {:s} {:s}'.format(urdf_path, srdf_path)
herb_name = urdf_module.SendCommand(args)
if herb_name is None:
raise ValueError('Failed loading HERB model using or_urdf.')
robot = env.GetRobot(herb_name)
if robot is None:
raise ValueError('Unable to find robot with name "{:s}".'.format(
herb_name))
else:
if robot_xml is None:
import os, rospkg
rospack = rospkg.RosPack()
base_path = rospack.get_path('herb_description')
robot_xml = os.path.join(base_path, 'ordata', 'robots', 'herb.robot.xml')
robot = env.ReadRobotXMLFile(robot_xml)
env.Add(robot)
# Default arguments.
keys = [ 'left_arm_sim', 'left_hand_sim', 'left_ft_sim',
'right_arm_sim', 'right_hand_sim', 'right_ft_sim',
'head_sim', 'talker_sim', 'segway_sim', 'perception_sim' ]
for key in keys:
if key not in kw_args:
kw_args[key] = sim
from herbrobot import HERBRobot
prpy.bind_subclass(robot, HERBRobot, **kw_args)
if sim:
dof_indices, dof_values \
= robot.configurations.get_configuration('relaxed_home')
robot.SetDOFValues(dof_values, dof_indices)
# Start by attempting to load or_rviz.
if attach_viewer == True:
attach_viewer = 'rviz'
env.SetViewer(attach_viewer)
# Fall back on qtcoin if loading or_rviz failed
if env.GetViewer() is None:
logger.warning(
'Loading the RViz viewer failed. Do you have or_interactive'
' marker installed? Falling back on qtcoin.')
attach_viewer = 'qtcoin'
if attach_viewer and env.GetViewer() is None:
env.SetViewer(attach_viewer)
if env.GetViewer() is None:
raise Exception('Failed creating viewer of type "{0:s}".'.format(
attach_viewer))
# Remove the ROS logging handler again. It might have been added when we
# loaded or_rviz.
prpy.logger.remove_ros_logger()
return env, robot
def __init__(self, left_arm_sim, right_arm_sim, right_ft_sim,
left_hand_sim, right_hand_sim, left_ft_sim,
head_sim, talker_sim, segway_sim, perception_sim):
from prpy.util import FindCatkinResource
Robot.__init__(self, robot_name='herb')
# Convenience attributes for accessing self components.
self.left_arm = self.GetManipulator('left')
self.right_arm = self.GetManipulator('right')
self.head = self.GetManipulator('head')
self.left_arm.hand = self.left_arm.GetEndEffector()
self.right_arm.hand = self.right_arm.GetEndEffector()
self.left_hand = self.left_arm.hand
self.right_hand = self.right_arm.hand
self.manipulators = [ self.left_arm, self.right_arm, self.head ]
# Dynamically switch to self-specific subclasses.
prpy.bind_subclass(self.left_arm, WAM, sim=left_arm_sim, owd_namespace='/left/owd')
prpy.bind_subclass(self.right_arm, WAM, sim=right_arm_sim, owd_namespace='/right/owd')
prpy.bind_subclass(self.head, HERBPantilt, sim=head_sim, owd_namespace='/head/owd')
prpy.bind_subclass(self.left_arm.hand, BarrettHand, sim=left_hand_sim, manipulator=self.left_arm,
owd_namespace='/left/owd', bhd_namespace='/left/bhd', ft_sim=right_ft_sim)
prpy.bind_subclass(self.right_arm.hand, BarrettHand, sim=right_hand_sim, manipulator=self.right_arm,
owd_namespace='/right/owd', bhd_namespace='/right/bhd', ft_sim=right_ft_sim)
self.base = HerbBase(sim=segway_sim, robot=self)
# Set HERB's acceleration limits. These are not specified in URDF.
accel_limits = self.GetDOFAccelerationLimits()
accel_limits[self.head.GetArmIndices()] = [ 2. ] * self.head.GetArmDOF()
accel_limits[self.left_arm.GetArmIndices()] = [ 2. ] * self.left_arm.GetArmDOF()
accel_limits[self.right_arm.GetArmIndices()] = [ 2. ] * self.right_arm.GetArmDOF()
self.SetDOFAccelerationLimits(accel_limits)
# Support for named configurations.
import os.path
self.configurations.add_group('left_arm', self.left_arm.GetArmIndices())
self.configurations.add_group('right_arm', self.right_arm.GetArmIndices())
self.configurations.add_group('head', self.head.GetArmIndices())
self.configurations.add_group('left_hand', self.left_hand.GetIndices())
self.configurations.add_group('right_hand', self.right_hand.GetIndices())
configurations_path = FindCatkinResource('herbpy', 'config/configurations.yaml')
try:
self.configurations.load_yaml(configurations_path)
except IOError as e:
raise ValueError('Failed laoding named configurations from "{:s}".'.format(
configurations_path))
# Initialize a default planning pipeline.
from prpy.planning import (
FirstSupported,
MethodMask,
Ranked,
Sequence,
)
from prpy.planning import (
CBiRRTPlanner,
CHOMPPlanner,
GreedyIKPlanner,
IKPlanner,
NamedPlanner,
SBPLPlanner,
SnapPlanner,
TSRPlanner,
VectorFieldPlanner
)
# TODO: These should be meta-planners.
self.named_planner = NamedPlanner()
self.ik_planner = IKPlanner()
# Special-purpose planners.
self.snap_planner = SnapPlanner()
self.vectorfield_planner = VectorFieldPlanner()
self.greedyik_planner = GreedyIKPlanner()
# General-purpose planners.
self.cbirrt_planner = CBiRRTPlanner()
# Trajectory optimizer.
try:
from or_trajopt import TrajoptPlanner
self.trajopt_planner = TrajoptPlanner()
except ImportError:
self.trajopt_planner = None
logger.warning('Failed creating TrajoptPlanner. Is the or_trajopt'
' package in your workspace and built?')
try:
self.chomp_planner = CHOMPPlanner()
except UnsupportedPlanningError:
self.chomp_planner = None
logger.warning('Failed loading the CHOMP module. Is the or_cdchomp'
' package in your workspace and built?')
if not (self.trajopt_planner or self.chomp_planner):
raise PrPyException('Unable to load both CHOMP and TrajOpt. At'
' least one of these packages is required.')
actual_planner = Sequence(
# First, try the straight-line trajectory.
self.snap_planner,
# Then, try a few simple (and fast!) heuristics.
self.vectorfield_planner,
#self.greedyik_planner,
# Next, try a trajectory optimizer.
self.trajopt_planner or self.chomp_planner
)
self.planner = FirstSupported(
Sequence(actual_planner,
TSRPlanner(delegate_planner=actual_planner),
self.cbirrt_planner),
# Special purpose meta-planner.
NamedPlanner(delegate_planner=actual_planner),
)
from prpy.planning.retimer import HauserParabolicSmoother
self.smoother = HauserParabolicSmoother(do_blend=True, do_shortcut=True)
self.retimer = HauserParabolicSmoother(do_blend=True, do_shortcut=False,
blend_iterations=5, blend_radius=0.4)
self.simplifier = None
# Base planning
from prpy.util import FindCatkinResource
planner_parameters_path = FindCatkinResource('herbpy', 'config/base_planner_parameters.yaml')
self.sbpl_planner = SBPLPlanner()
try:
with open(planner_parameters_path, 'rb') as config_file:
import yaml
params_yaml = yaml.load(config_file)
self.sbpl_planner.SetPlannerParameters(params_yaml)
except IOError as e:
raise ValueError('Failed loading base planner parameters from "{:s}".'.format(
planner_parameters_path))
self.base_planner = self.sbpl_planner
# Create action library
from prpy.action import ActionLibrary
self.actions = ActionLibrary()
# Register default actions and TSRs
import herbpy.action
import herbpy.tsr
# Setting necessary sim flags
self.talker_simulated = talker_sim
self.segway_sim = segway_sim
# Set up perception
self.detector=None
if perception_sim:
from prpy.perception import SimulatedPerceptionModule
self.detector = SimulatedPerceptionModule()
else:
from prpy.perception import ApriltagsModule
try:
kinbody_path = prpy.util.FindCatkinResource('pr_ordata',
'data/objects')
marker_data_path = prpy.util.FindCatkinResource('pr_ordata',
'data/objects/tag_data.json')
self.detector = ApriltagsModule(marker_topic='/apriltags_kinect2/marker_array',
marker_data_path=marker_data_path,
kinbody_path=kinbody_path,
detection_frame='head/kinect2_rgb_optical_frame',
destination_frame='map')
except IOError as e:
logger.warning('Failed to find required resource path. ' \
'pr-ordata package cannot be found. ' \
'Perception detector will not be loaded.')
if not self.talker_simulated:
# Initialize herbpy ROS Node
import rospy
if not rospy.core.is_initialized():
rospy.init_node('herbpy', anonymous=True)
logger.debug('Started ROS node with name "%s".', rospy.get_name())
import talker.msg
from actionlib import SimpleActionClient
self._say_action_client = SimpleActionClient('say', talker.msg.SayAction)
def __init__(self, sim):
""" Create an ADARobot.
@param sim: Set to True to run the robot in simulation.
"""
from mico import Mico
from micohand import MicoHand
from util import AdaPyException, find_adapy_resource
self.simulated = sim
self.talker_simulated = sim
# We need to hard-code the name. Otherwise, it defaults to
# "mico-modified".
Robot.__init__(self, robot_name='ada')
# Convenience attributes for accessing self components.
self.arm = self.GetManipulator('Mico')
self.arm.hand = self.arm.GetEndEffector()
self.manipulators = [ self.arm ]
# Bind robot-specific subclasses.
prpy.bind_subclass(self.arm, Mico, sim=sim)
prpy.bind_subclass(self.arm.hand, MicoHand, sim=sim,
manipulator=self.arm)
# TODO: Load an IdealController controller in simulation.
# Load default named configurations from YAML.
self.configurations.add_group('arm', self.arm.GetIndices())
try:
self.configurations.load_yaml(
find_adapy_resource(CONFIGURATIONS_PATH))
except IOError as e:
logger.warning('Failed loading named configurations from "%s": %s',
configurations_path, e.message)
# If in sim, set the robot DOFs to not be in collision
if sim:
inds,dofs = self.configurations.get_configuration('home')
self.SetDOFValues(values=dofs,dofindices=inds)
# Load default TSRs from YAML.
for tsr_path_relative in TSR_PATHS:
try:
tsr_path = find_adapy_resource(tsr_path_relative)
self.tsrlibrary.load_yaml(tsr_path)
except IOError as e:
raise ValueError(
'Failed loading TSRs from "{:s}": {:s}'.format(
tsr_path, e.message))
# Create ros_control clients for execution on real hardware.
if not sim:
from .controller_client import (ControllerManagerClient,
JointStateClient)
from .trajectory_client import FollowJointTrajectoryClient
self._trajectory_joint_names = [
self.GetJointFromDOFIndex(i).GetName()
for i in xrange(self.GetDOF())
]
self._jointstate_client = JointStateClient(
self, topic_name='/joint_states'
)
self._controller_client = ControllerManagerClient(
ns='/controller_manager'
)
self._velocity_joint_mode_controller = self._controller_client.request(
'velocity_joint_mode_controller'
)
self._velocity_joint_mode_controller.switch()
self._trajectory_switcher = self._controller_client.request(
'traj_controller'
)
self._trajectory_client = FollowJointTrajectoryClient(
ns='/traj_controller/follow_joint_trajectory'
)
# Initialize the default planning pipeline.
from prpy.planning import Sequence, Ranked, FirstSupported
from prpy.planning import (
BiRRTPlanner,
CBiRRTPlanner,
IKPlanner,
GreedyIKPlanner,
NamedPlanner,
SBPLPlanner,
SnapPlanner,
IKPlanner,
TSRPlanner,
VectorFieldPlanner
)
self.snap_planner = SnapPlanner()
self.greedyik_planner = GreedyIKPlanner()
self.cbirrt_planner = CBiRRTPlanner()
self.vectorfield_planner = VectorFieldPlanner()
# Hide TrajOpt logging.
os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN')
# Trajectory optimizer.
try:
from or_trajopt import TrajoptPlanner
self.trajopt_planner = TrajoptPlanner()
except ImportError:
self.trajopt_planner = None
logger.warning('Failed creating TrajoptPlanner. Is the or_trajopt'
' package in your workspace and built?')
ik_planners = Sequence(
self.snap_planner,
self.trajopt_planner,
)
planner_for_ik = FirstSupported(
ik_planners,
NamedPlanner(delegate_planner=ik_planners)
)
self.ik_planner = IKPlanner(delegate_planner=planner_for_ik)
actual_planner = Sequence(
self.snap_planner,
self.ik_planner,
self.trajopt_planner,
self.vectorfield_planner,
#self.greedyik_planner,
self.cbirrt_planner
)
self.planner = FirstSupported(
actual_planner,
NamedPlanner(delegate_planner=actual_planner)
)
self.simplifier = None
#from prpy.planning.retimer import ParabolicSmoother, HauserParabolicSmoother
# self.smoother = Sequence(
# ParabolicSmoother(),
# HauserParabolicSmoother()
# )
from prpy.planning.retimer import HauserParabolicSmoother
self.smoother = HauserParabolicSmoother(do_blend=True, do_shortcut=True)
self.retimer = HauserParabolicSmoother(do_blend=True, do_shortcut=False)
from prpy.action import ActionLibrary
self.actions = ActionLibrary()
if not self.talker_simulated:
# Initialize herbpy ROS Node
import rospy
if not rospy.core.is_initialized():
rospy.init_node('adapy', anonymous=True)
logger.debug('Started ROS node with name "%s".', rospy.get_name())
def __init__(self, arm_sim, arm_torso_sim, gripper_sim, head_sim,
torso_sim, base_sim, talker_sim, whole_body_sim,
perception_sim, robot_checker_factory):
Robot.__init__(self, robot_name='fetch')
self.robot_checker_factory = robot_checker_factory
#Controller Setup
self.controller_manager = None
self.controller_always_on = []
self.full_controller_sim = (arm_sim and arm_torso_sim and gripper_sim
and head_sim)
if not self.full_controller_sim:
import rospy
from ros_control_client_py import (
ControllerManagerClient,
JointStateClient,
)
import rosgraph.masterapi
if not rospy.core.is_initialized():
raise RuntimeError('rospy not initialized.'
'Must call rospy.init_node()')
master = rosgraph.masterapi.Master('/rostopic')
topics = []
for i in master.getPublishedTopics(''):
topics.append(i[0])
if '/joint_states' not in topics:
raise RuntimeError(
'The -sim argument should be set if there is'
' No real robot/Gazebo robot connected')
#update openrave state from /joint_states
self._jointstate_client = JointStateClient(
self, topic_name='/joint_states')
self.controller_always_on.append('joint_state_controller')
# Convenience attributes for accessing self components.
self.arm = self.GetManipulator('arm')
self.arm_torso = self.GetManipulator('arm_torso')
self.gripper = self.arm.GetEndEffector()
self.hand = self.gripper
self.head = self.GetManipulator('head')
self.manipulators = [self.arm, self.arm_torso]
#Dynamically switch to self-specific subclasses.
prpy.bind_subclass(self.arm, ARM, sim=arm_sim, namespace='arm')
prpy.bind_subclass(self.arm_torso,
ARM,
sim=arm_torso_sim,
namespace='arm_torso')
prpy.bind_subclass(self.gripper,
GRIPPER,
sim=gripper_sim,
manipulator=self.arm,
namespace='')
prpy.bind_subclass(self.head,
HEAD,
robot=self,
sim=head_sim,
namespace='')
self.base = BASE(sim=base_sim, robot=self)
self.whole_body = WholeBody(sim=whole_body_sim, robot=self)
# Set FETCH's acceleration limits. These are not specified in URDF.
accel_limits = self.GetDOFAccelerationLimits()
accel_limits[self.arm.GetArmIndices()] = [2.] * self.arm.GetArmDOF()
accel_limits[self.arm_torso.GetArmIndices(
)] = [2.] * self.arm_torso.GetArmDOF()
#get acceleration limits and set them here(change)
self.SetDOFAccelerationLimits(accel_limits)
# Determine always-on controllers
# Set default manipulator controllers in sim only (change)
if not gripper_sim:
self.controller_always_on.append('gripper_controller')
if not head_sim:
self.controller_always_on.append('head_controller')
if not base_sim:
self.controller_always_on.append('base_controller')
if arm_sim:
self.arm.sim_controller = self.AttachController(
name=self.arm.GetName(),
args='IdealController',
dof_indices=self.arm.GetArmIndices(),
affine_dofs=0,
simulated=True)
# For simulation we cannot assign same DOF indices to different controllers
# So for arm_torso only the torso is included
if arm_torso_sim:
self.arm_torso.sim_controller = self.AttachController(
name=self.arm_torso.GetName(),
args='IdealController',
dof_indices=[11],
affine_dofs=0,
simulated=True)
# Support for named configurations.(change)
import os.path
self.configurations.add_group('arm', self.arm.GetArmIndices())
#self.configurations.add_group('arm_torso',self.arm.GetArmIndices())
self.configurations.add_group('gripper', self.gripper.GetIndices())
self.configurations.add_group('head', self.head.GetIndices())
configurations_path = FindCatkinResource('fetchpy',
'config/configurations.yaml')
try:
self.configurations.load_yaml(configurations_path)
except IOError as e:
raise ValueError(
'Failed loading named configurations from "{:s}".'.format(
configurations_path))
#Gripper configurations
self.gripper.configurations = ConfigurationLibrary()
self.gripper.configurations.add_group('gripper',
self.gripper.GetIndices())
if isinstance(self.hand, GRIPPER):
gripper_configurations_path = FindCatkinResource(
'fetchpy', 'config/gripper_preshapes.yaml')
try:
self.configurations.load_yaml(gripper_configurations_path)
except IOError as e:
raise ValueError(
'Failed loading named configurations from "{:s}".'.format(
gripper_configurations_path))
else:
logger.warning(
'Unrecognized hand class. Not loading named configurations.')
#Head configurations
self.head.configurations = ConfigurationLibrary()
self.head.configurations.add_group('head', self.head.GetIndices())
if isinstance(self.head, HEAD):
head_configurations_path = FindCatkinResource(
'fetchpy', 'config/head_preshapes.yaml')
try:
self.configurations.load_yaml(head_configurations_path)
except IOError as e:
raise ValueError(
'Failed loading named configurations from "{:s}".'.format(
head_configurations_path))
else:
logger.warning(
'Unrecognized HEAD class. Not loading named configurations.')
#Planner.
snap_planner = SnapPlanner(
robot_checker_factory=self.robot_checker_factory)
vectorfield_planner = VectorFieldPlanner(
robot_checker_factory=self.robot_checker_factory)
trajopt_planner = TrajoptPlanner(
robot_checker_factory=self.robot_checker_factory)
rrt_planner = OMPLPlanner(
'RRTConnect', robot_checker_factory=self.robot_checker_factory)
cbirrt_planner = CBiRRTPlanner(
timelimit=1., robot_checker_factory=self.robot_checker_factory)
actual_planner = Sequence(
snap_planner,
vectorfield_planner,
trajopt_planner,
TSRPlanner(delegate_planner=Sequence(snap_planner,
trajopt_planner),
robot_checker_factory=self.robot_checker_factory),
FirstSupported(rrt_planner, cbirrt_planner),
)
self.planner = FirstSupported(
actual_planner,
NamedPlanner(delegate_planner=actual_planner),
)
# Post-processor.
self.smoother = HauserParabolicSmoother(do_blend=True,
blend_iterations=1,
blend_radius=0.4,
do_shortcut=True,
timelimit=0.6)
self.retimer = HauserParabolicSmoother(do_blend=True,
blend_iterations=1,
blend_radius=0.4,
do_shortcut=False)
self.simplifier = None
#Base Planning
self.sbpl_planner = SBPLPlanner()
self.base_planner = self.sbpl_planner
#Create action library(change)
self.actions = ActionLibrary()
# Register default actions and TSRs: TODO(change)
import fetchpy.action
#import fetch.tsr
#Setting necessary sim flags
self.talker_simulated = talker_sim
self.base_sim = base_sim
#Set up perception(change)
self.detector = None
if perception_sim:
from prpy.perception import SimulatedPerceptionModule
self.detector = SimulatedPerceptionModule()
else:
from prpy.perception import ApriltagsModule
try:
kinbody_path = FindCatkinResource('pr_ordata', 'data/objects')
marker_data_path = FindCatkinResource(
'pr_ordata', 'data/objects/tag_data.json')
self.detector = ApriltagsModule(
marker_topic='/apriltags_kinect2/marker_array',
marker_data_path=marker_data_path,
kinbody_path=kinbody_path,
detection_frame='head/kinect2_rgb_optical_frame',
destination_frame='fetch_base',
reference_link=self.GetLink('/base'))
except IOError as e:
logger.warning('Failed to find required resource path. ' \
'pr_ordata package cannot be found. ' \
'Perception detector will not be loaded.' \
'\n{}'.format(e))
#TALKER
if not self.talker_simulated:
# Initialize fetchpy ROS Node
if not rospy.core.is_initialized():
raise RuntimeError('rospy not initialized. '
'Must call rospy.init_node()')
from sound_play.msg import SoundRequest
from sound_play.libsoundplay import SoundClient
self.soundhandle = SoundClient()
self.voice = 'voice_kal_diphone'
self.volume = 1.0
def initialize(robot_xml=None, env_path=None, attach_viewer=False,
sim=True, **kw_args):
import prpy, os
prpy.logger.initialize_logging()
# Hide TrajOpt logging.
os.environ.setdefault('TRAJOPT_LOG_THRESH', 'WARN')
# Load plugins.
if prpy.dependency_manager.is_catkin():
prpy.dependency_manager.export()
else:
prpy.dependency_manager.export(PACKAGE)
openravepy.RaveInitialize(True)
# Create the environment.
env = openravepy.Environment()
if env_path is not None:
if not env.Load(env_path):
raise Exception('Unable to load environment frompath %s' % env_path)
# Load the robot
model_name = None
share_directories = None
if robot_xml is None:
if prpy.dependency_manager.is_catkin():
from catkin.find_in_workspaces import find_in_workspaces
share_directories = find_in_workspaces(search_dirs=['share'],
project=PACKAGE)
if not share_directories:
logger.error('Unable to find share directory in catkin workspace.')
raise ValueError('Unable to find share directory in catkin workspace.')
import os.path
model_name = 'barrettsegway.robot.xml'
for share_directory in share_directories:
xml_path = os.path.join(share_directory, 'robots', model_name)
if os.path.exists(xml_path):
robot_xml = xml_path
if robot_xml is None:
err_str = 'Unable to find robot model with name: %s in directories %s.' % (model_name, share_directories)
logger.error(err_str)
raise ValueError(err_str)
# Load the robot model
robot = env.ReadRobotXMLFile(robot_xml)
env.Add(robot)
# Default arguments.
keys = [ 'arm_sim',
'hand_sim',
'ft_sim',
'segway_sim'
]
for key in keys:
if key not in kw_args:
kw_args[key] = sim
from wamrobot import WAMRobot
prpy.bind_subclass(robot, WAMRobot, **kw_args)
if sim:
dof_indices, dof_values \
= robot.configurations.get_configuration('zero')
robot.SetDOFValues(dof_values, dof_indices)
# Start by attempting to load or_rviz.
if attach_viewer == True:
attach_viewer = 'rviz'
env.SetViewer(attach_viewer)
# Fall back on qtcoin if loading or_rviz failed
if env.GetViewer() is None:
logger.warning(
'Loading the RViz viewer failed. Do you have or_interactive'
' marker installed? Falling back on qtcoin.')
attach_viewer = 'qtcoin'
if attach_viewer and env.GetViewer() is None:
env.SetViewer(attach_viewer)
if env.GetViewer() is None:
raise Exception('Failed creating viewer of type "{0:s}".'.format(
attach_viewer))
# Remove the ROS logging handler again.
# It might have been added when we loaded or_rviz.
prpy.logger.remove_ros_logger()
return env, robot