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 __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 __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 ExecuteTrajectory(self, traj, defer=False, timeout=None, switch=True,
unswitch=None, **kwargs):
""" Executes a time trajectory on the robot.
This function directly executes a timed OpenRAVE trajectory on the
robot. If you have a geometric path, such as those returned by a
geometric motion planner, you should first time the path using
PostProcessPath. Alternatively, you could use the ExecutePath helper
function to time and execute the path in one function call.
If timeout = None (the default), this function does not return until
execution has finished. Termination occurs if the trajectory is
successfully executed or if a fault occurs (in this case, an exception
will be raised). If timeout is a float (including timeout = 0), this
function will return None once the timeout has ellapsed, even if the
trajectory is still being executed.
NOTE: We suggest that you either use timeout=None or defer=True. If
trajectory execution times out, there is no way to tell whether
execution was successful or not. Other values of timeout are only
supported for legacy reasons.
This function returns the trajectory that was actually executed on the
robot, including controller error. If this is not available, the input
trajectory will be returned instead.
If switch = True, this function switches to the ros_control controllers
necessary to execute traj. If unswitch = True, it also undoes this
switch after the trajectory has finished executing. If unswitch is
unspecified, it defaults to the same value as switch.
@param traj: timed trajectory to execute
@type traj: openravepy.Trajectory
@param defer: execute asynchronously and return a trajectory Future
@type defer: bool
@param timeout: maximum time to wait for execution to finish
@type timeout: float or None
@param switch: switch to the controllers necessary to execute traj
@type switch: bool
@param unswitch: revert the controllers after executing traj
@type unswitch: bool or None
@return trajectory executed on the robot
@rtype openravepy.Trajectory or TrajectoryFuture
"""
from .util import or_to_ros_trajectory, pad_ros_trajectory
from rospy import Time
from prpy import exceptions
if self.simulated:
return Robot.ExecuteTrajectory(self, traj, defer=defer, timeout=timeout,
**kwargs)
# Check that the current configuration of the robot matches the
# initial configuration specified by the trajectory.
# if not prpy.util.IsAtTrajectoryStart(self, traj):
# raise exceptions.TrajectoryNotExecutable(
# 'Trajectory started from different configuration than robot.')
# If there was only one waypoint, at this point we are done!
if traj.GetNumWaypoints() == 1:
return traj
if unswitch is None:
unswitch = switch
traj_msg = or_to_ros_trajectory(self, traj)
# sometimes points are removed from message
# ensure this trajectory is useful to send before continuing
if len(traj_msg.points) < 1:
return traj
# The trajectory_controller expects the full set of DOFs to be
# specified in every trajectory. We pad the trajectory by adding
# constant values for any missing joints.
#print traj_msg
#print "\n\n\n\n\n\n"
#print traj.serialize()
#from IPython import embed
#embed()
with self.GetEnv():
pad_ros_trajectory(self, traj_msg, self._trajectory_joint_names)
if switch:
self._trajectory_switcher.switch()
traj_future = self._trajectory_client.execute(traj_msg)
if unswitch:
traj_future.add_done_callback(
lambda _: self._trajectory_switcher.unswitch
)
#for i in range(0,len(traj_msg.points)):
# print traj_msg.points[i].positions
# print '\n'
from trajectory_client import TrajectoryExecutionFailed
if defer:
return traj_future
else:
try:
traj_future.result(timeout)
return traj
except TrajectoryExecutionFailed as e:
logger.exception('Trajectory execution failed.')
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
