def __init__(self, planner, robot_checker_factory=None):
"""
Create a PrPy binding that wraps an existing planner and calls
its planning methods followed by Trajopt's OptimizeTrajectory.
@param planner the PrPy plan wrapper that will be wrapped
"""
assert planner
if robot_checker_factory is None:
robot_checker_factory = DefaultRobotCollisionCheckerFactory
# TODO: this should be revisited once the MetaPlanners are not assuming
# self._planners must exist.
self._planners = (planner,)
self._trajopt = TrajoptPlanner(
robot_checker_factory=robot_checker_factory)
self._simplifier = HauserParabolicSmoother(timelimit=0.25)
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)
class TrajoptWrapper(MetaPlanner):
def __init__(self, planner, robot_checker_factory=None):
"""
Create a PrPy binding that wraps an existing planner and calls
its planning methods followed by Trajopt's OptimizeTrajectory.
@param planner the PrPy plan wrapper that will be wrapped
"""
assert planner
if robot_checker_factory is None:
robot_checker_factory = DefaultRobotCollisionCheckerFactory
# TODO: this should be revisited once the MetaPlanners are not assuming
# self._planners must exist.
self._planners = (planner,)
self._trajopt = TrajoptPlanner(
robot_checker_factory=robot_checker_factory)
self._simplifier = HauserParabolicSmoother(timelimit=0.25)
def __str__(self):
return 'TrajoptWrapper({0:s})'.format(self._planners[0])
def get_planners(self, method_name):
return [planner for planner in self._planners
if hasattr(planner, method_name)]
def plan(self, method, args, kwargs):
# According to prpy spec, the first positional argument is 'robot'.
robot = args[0]
# Call the wrapped planner to get the seed trajectory.
planner_method = getattr(self._planners[0], method)
traj = planner_method(*args, **kwargs)
# Simplify redundant waypoints in the trajectory.
from prpy.util import SimplifyTrajectory
traj = SimplifyTrajectory(traj, robot)
# Run path shortcutting on the RRT path.
traj = self._simplifier.RetimeTrajectory(robot, traj)
# Call Trajopt to optimize the seed trajectory.
# Try different distance penalties until out of collision.
penalties = numpy.logspace(numpy.log(0.05), numpy.log(0.5), num=4)
for distance_penalty in penalties:
try:
return self._trajopt.OptimizeTrajectory(
robot, traj,
**kwargs
)
except PlanningError:
logger.warn("Failed to optimize trajectory "
"with distance penalty: {:f}"
.format(distance_penalty))
# If all of the optimizations ended in collision,
# just return the original.
logger.warn("Failed to optimize trajectory, "
"returning unoptimized solution.")
return traj
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())
class HauserParabolicSmootherRetimingTests(BasePlannerTest,
RetimeTrajectoryTest, TestCase):
planner_factory = lambda _: HauserParabolicSmoother(do_blend=False,
do_shortcut=False)
class HauserParabolicSmootherBlendingTests(BasePlannerTest,
SmoothTrajectoryTest, TestCase):
planner_factory = lambda _: HauserParabolicSmoother(do_blend=True,
do_shortcut=False)
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 RetimeWholeBodyTrajectory(robot, arm_traj, base_traj):
affine_retimer = OpenRAVEAffineRetimer()
retimer = ParabolicRetimer()
smoother = HauserParabolicSmoother()
simplifier = None
env = arm_traj.GetEnv()
cspec = arm_traj.GetConfigurationSpecification()
dof_indices, _ = cspec.ExtractUsedIndices(robot)
affine_dofs = (DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis)
#_postprocess_envs = collections.defaultdict(openravepy.Environment)
'''Separating base and arm trajectory and try to retime them individually.
As openrave does not support retime both of the affine and joint_dof at same time
There will be issues.As affine retiming keeps the same number of points while
joint_dof retiming reduces number of points.'''
postprocess_envs = robot._postprocess_envs[openravepy.RaveGetEnvironmentId(env)]
retiming_options = dict()
affine_retimer_options = dict()
with Clone(env, clone_env = postprocess_envs) as cloned_env:
cloned_robot1 = cloned_env.Cloned(robot)
cloned_robot1.SetActiveDOFs(dof_indices)
arm_timed_traj = retimer.RetimeTrajectory(cloned_robot1, arm_traj, **retiming_options)
arm_final_traj = CopyTrajectory(arm_timed_traj, env=env)
cloned_robot2 = cloned_env.Cloned(robot)
cloned_robot2.SetActiveDOFs([],affine_dofs)
base_timed_traj = affine_retimer.RetimeTrajectory(cloned_robot2, base_traj, **affine_retimer_options)
base_final_traj = CopyTrajectory(base_timed_traj, env = env)
'''Creating a whole body trajectory from arm_timed and base_timed traj
The timestamp and number of waypoints are going to be different from both the trajectories
so if the arm_timed_traj has higher number of waypoints, after the waypoints from
base are over, every other waypoint will receive zeros for position and velocity and if the
base_timed_traj has higher number of waypoints, then all the arm waypoints will receive
the values of last waypoint. Openrave trajectory format now only can receive one deltatime.
(Hacky way) as we dont use the affine velocity, the base time is merged in affine_velocity.
Thats why it is 4'''
base_num_of_way = base_final_traj.GetNumWaypoints()
arm_num_of_way = arm_final_traj.GetNumWaypoints()
print 'base has '+str(base_num_of_way)+' waypoints'
print 'arm has '+str(arm_num_of_way)+' waypoints'
a_cspec = arm_final_traj.GetConfigurationSpecification()
b_cspec = base_final_traj.GetConfigurationSpecification()
with env:
robot.SetActiveDOFs(dof_indices, affine_dofs)
cspec = robot.GetActiveConfigurationSpecification('linear')
traj = openravepy.RaveCreateTrajectory(env, '')
cspec.AddGroup('joint_velocities', dof= 8, interpolation='quadratic')
cspec.AddGroup('affine_velocities', dof= 4, interpolation='next')
cspec.AddDeltaTimeGroup()
traj.Init(cspec)
highest_way = higherNumber(base_num_of_way, arm_num_of_way)
arm_final_waypoint = []
for i in range(highest_way):
value = []
arm_waypoint = []
base_waypoint = []
dt = 0.0
if(i>=base_num_of_way):
arm_waypoint = arm_final_traj.GetWaypoint(i)
base_waypoint = np.zeros(7)
dt = arm_waypoint[-1]
elif(i>=arm_num_of_way):
arm_waypoint = arm_final_waypoint
base_waypoint = base_final_traj.GetWaypoint(i)
dt = base_waypoint[-1]
else:
arm_waypoint = arm_final_traj.GetWaypoint(i)
base_waypoint = base_final_traj.GetWaypoint(i)
dt = arm_waypoint[-1]
arm_way_point = arm_waypoint[:-1]
value.extend(arm_way_point[:8])
value.extend(base_waypoint[:3])
value.extend(arm_way_point[8:])
value.extend(base_waypoint[3:])
value.extend([dt])
traj.Insert(i, value)
arm_final_waypoint = arm_waypoint
return traj