class HERBRobot(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 CloneBindings(self, parent):
super(HERBRobot, self).CloneBindings(parent)
self.left_arm = Cloned(parent.left_arm)
self.right_arm = Cloned(parent.right_arm)
self.head = Cloned(parent.head)
self.left_arm.hand = Cloned(parent.left_arm.GetEndEffector())
self.right_arm.hand = Cloned(parent.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]
self.planner = parent.planner
self.base_planner = parent.base_planner
def _ExecuteTrajectory(self,
traj,
defer=False,
timeout=None,
period=0.01,
**kwargs):
if defer is not False:
raise RuntimeError('defer functionality was deprecated in '
'personalrobotics/prpy#278')
# Don't execute trajectories that don't have at least one waypoint.
if traj.GetNumWaypoints() <= 0:
raise ValueError('Trajectory must contain at least one waypoint.')
# Check if this trajectory contains both affine and joint DOFs
cspec = traj.GetConfigurationSpecification()
needs_base = prpy.util.HasAffineDOFs(cspec)
needs_joints = prpy.util.HasJointDOFs(cspec)
if needs_base and needs_joints:
raise ValueError(
'Trajectories with affine and joint DOFs are not supported')
# Check that the current configuration of the robot matches the
# initial configuration specified by the trajectory.
if not prpy.util.IsAtTrajectoryStart(self, traj):
raise 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
# Verify that the trajectory is timed by checking whether the first
# waypoint has a valid deltatime value.
if not prpy.util.IsTimedTrajectory(traj):
raise ValueError('Trajectory cannot be executed, it is not timed.')
# Verify that the trajectory has non-zero duration.
if traj.GetDuration() <= 0.0:
logger.warning(
'Executing zero-length trajectory. Please update the'
' function that produced this trajectory to return a'
' single-waypoint trajectory.', FutureWarning)
traj_manipulators = self.GetTrajectoryManipulators(traj)
controllers_manip = []
active_controllers = []
if self.head in traj_manipulators:
# TODO head after Schunk integration
if len(traj_manipulators) == 1:
raise NotImplementedError(
'The head is currently disabled under ros_control.')
else:
logger.warning(
'The head is currently disabled under ros_control.')
# logic to determine which controllers are needed
if (self.right_arm in traj_manipulators
and not self.right_arm.IsSimulated()
and self.left_arm in traj_manipulators
and not self.left_arm.IsSimulated()):
controllers_manip.append('bimanual_trajectory_controller')
else:
if self.right_arm in traj_manipulators:
if not self.right_arm.IsSimulated():
controllers_manip.append('right_trajectory_controller')
else:
active_controllers.append(self.right_arm.sim_controller)
if self.left_arm in traj_manipulators:
if not self.left_arm.IsSimulated():
controllers_manip.append('left_trajectory_controller')
else:
active_controllers.append(self.left_arm.sim_controller)
# load and activate controllers
if not self.full_controller_sim:
self.controller_manager.request(controllers_manip).switch()
# repeat logic and actually construct controller clients
# now that we've activated them on the robot
if 'bimanual_trajectory_controller' in controllers_manip:
joints = []
joints.extend(self.right_arm.GetJointNames())
joints.extend(self.left_arm.GetJointNames())
active_controllers.append(
RewdOrTrajectoryController(self, '',
'bimanual_trajectory_controller',
joints))
else:
if 'right_trajectory_controller' in controllers_manip:
active_controllers.append(
RewdOrTrajectoryController(self, '',
'right_trajectory_controller',
self.right_arm.GetJointNames()))
if 'left_trajectory_controller' in controllers_manip:
active_controllers.append(
RewdOrTrajectoryController(self, '',
'left_trajectory_controller',
self.left_arm.GetJointNames()))
if needs_base:
if (hasattr(self, 'base') and hasattr(self.base, 'controller')
and self.base.controller is not None):
active_controllers.append(self.base.controller)
else:
logger.warning(
'Trajectory includes the base, but no base controller is'
' available. Is self.base.controller set?')
for controller in active_controllers:
controller.SetPath(traj)
prpy.util.WaitForControllers(active_controllers, timeout=timeout)
return traj
def ExecuteTrajectory(self, traj, *args, **kwargs):
# from prpy.exceptions import TrajectoryAborted
value = self._ExecuteTrajectory(traj, *args, **kwargs)
# TODO meaningful to do this check here?
# if so can be done on value future
#
# for manipulator in active_manipulators:
# status = manipulator.GetTrajectoryStatus()
# if status == 'aborted':
# raise TrajectoryAborted()
return value
# Inherit docstring from the parent class.
ExecuteTrajectory.__doc__ = Robot.ExecuteTrajectory.__doc__
def SetStiffness(self, stiffness, manip=None):
"""Set the stiffness of HERB's arms and head.
Stiffness False/0 is gravity compensation and stiffness True/(>0) is position
control.
@param stiffness boolean or numeric value 0.0 to 1.0
"""
if (isinstance(stiffness, numbers.Number)
and not (0 <= stiffness and stiffness <= 1)):
raise Exception(
'Stiffness must be boolean or numeric in the range [0, 1];'
'got {}.'.format(stiffness))
# TODO head after Schunk integration
if manip is self.head:
raise NotImplementedError(
'Head immobilized under ros_control, SetStiffness not available.'
)
new_manip_controllers = []
if stiffness:
if not self.left_arm.IsSimulated() and (manip is None
or manip is self.left_arm):
new_manip_controllers.append(
'left_joint_group_position_controller')
if not self.right_arm.IsSimulated() and (manip is None or
manip is self.right_arm):
new_manip_controllers.append(
'right_joint_group_position_controller')
else:
if not self.left_arm.IsSimulated() and (manip is None
or manip is self.left_arm):
new_manip_controllers.append(
'left_gravity_compensation_controller')
if not self.right_arm.IsSimulated() and (manip is None or
manip is self.right_arm):
new_manip_controllers.append(
'right_gravity_compensation_controller')
if not self.full_controller_sim:
self.controller_manager.request(new_manip_controllers).switch()
def Say(self, words, block=True):
"""Speak 'words' using talker action service or espeak locally in simulation"""
if self.talker_simulated:
try:
proc = subprocess.Popen(
['espeak', '-s', '160', '"{0}"'.format(words)])
if block:
proc.wait()
except OSError as e:
logger.error(
'Unable to speak. Make sure "espeak" is installed locally.\n%s'
% str(e))
else:
import talker.msg
goal = talker.msg.SayGoal(text=words)
self._say_action_client.send_goal(goal)
if block:
self._say_action_client.wait_for_result()
class FETCHRobot(Robot):
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 CloneBindings(self, parent):
super(FETCHRobot, self).CloneBindings(parent)
self.arm = Cloned(parent.arm)
self.arm_torso = Cloned(parent.arm_torso)
self.manipulators = [self.arm, self.arm_torso]
self.hand = Cloned(parent.arm.GetEndEffector())
self.gripper = self.hand
self.head = Cloned(parent.head)
self.planner = parent.planner
self.base_planner = parent.base_planner
def _ExecuteTrajectory(self,
traj,
defer=False,
timeout=None,
period=0.01,
**kwargs):
if defer is not False:
raise RuntimeError('defer functionality was deprecated in '
'personalrobotics/prpy#278')
# Don't execute trajectories that don't have at least one waypoint.
if traj.GetNumWaypoints() <= 0:
raise ValueError('Trajectory must contain at least one waypoint.')
# Check if this trajectory contains both affine and joint DOFs
cspec = traj.GetConfigurationSpecification()
needs_base = prpy.util.HasAffineDOFs(cspec)
needs_joints = prpy.util.HasJointDOFs(cspec)
#if needs_base and needs_joints:
#raise ValueError('Trajectories with affine and joint DOFs are not supported')
# Check that the current configuration of the robot matches the
# initial configuration specified by the trajectory.
if not prpy.util.IsAtTrajectoryStart(self, traj):
raise 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
# Verify that the trajectory is timed by checking whether the first
# waypoint has a valid deltatime value.
if not prpy.util.IsTimedTrajectory(traj):
raise ValueError('Trajectory cannot be executed, it is not timed.')
# Verify that the trajectory has non-zero duration.
if traj.GetDuration() <= 0.0:
logger.warning(
'Executing zero-length trajectory. Please update the'
' function that produced this trajectory to return a'
' single-waypoint trajectory.', FutureWarning)
traj_manipulators = self.GetTrajectoryManipulators(traj)
controllers_manip = []
active_controllers = []
#Implementing different logic to determine which manipulator we want
if (needs_joints) and not needs_base:
if 11 in prpy.util.GetTrajectoryIndices(traj):
#11 is DOF index of toso lift joint
if not self.arm_torso.IsSimulated():
controllers_manip.append('arm_with_torso_controller')
else:
active_controllers.append(self.arm_torso.sim_controller)
active_controllers.append(self.arm.sim_controller)
else:
if not self.arm.IsSimulated():
controllers_manip.append('arm_controller')
else:
active_controllers.append(self.arm.sim_controller)
# repeat logic and actually construct controller clients
# now that we've activated them on the robot
if 'arm_controller' in controllers_manip:
active_controllers.append(
RewdOrTrajectoryController(self, '', 'arm_controller',
self.arm.GetJointNames()))
if 'arm_with_torso_controller' in controllers_manip:
active_controllers.append(
RewdOrTrajectoryController(self, '',
'arm_with_torso_controller',
self.arm_torso.GetJointNames()))
if needs_base and not needs_joints:
if (hasattr(self, 'base') and hasattr(self.base, 'controller')
and self.base.controller is not None):
active_controllers.append(self.base.controller)
else:
logger.warning(
'Trajectory includes the base, but no base controller is'
'available. Is self.base.controller set?')
#For whole body (arm+torso+base)
if needs_base and needs_joints:
#print ' we are in whole body control. Yeah'
if self.whole_body.IsSimulated():
active_controllers.append(self.arm_torso.sim_controller)
active_controllers.append(self.arm.sim_controller)
active_controllers.append(self.base.controller)
else:
active_controllers.append(self.whole_body.controller)
##ADD HERE ALL THE CONTROLLERS (change)
for controller in active_controllers:
controller.SetPath(traj)
prpy.util.WaitForControllers(active_controllers, timeout=timeout)
return traj
def ExecuteTrajectory(self, traj, *args, **kwargs):
value = self._ExecuteTrajectory(traj, *args, **kwargs)
return value
# Inherit docstring from the parent class.
ExecuteTrajectory.__doc__ = Robot.ExecuteTrajectory.__doc__
def SetStiffness(self, stiffness, manip=None):
"""Set the stiffness of HERB's arms and head.
Stiffness False/0 is gravity compensation and stiffness True/(>0) is position
control.
@param stiffness boolean or numeric value 0.0 to 1.0
"""
raise NotImplementedError(
'Not Implemented yet AGAIN.'
'There is a gravity compensation controller on real robot running internally.'
)
def Say(self, words, block=True):
"""Speak 'words' using sound_play or espeak locally in simulation"""
if self.talker_simulated:
try:
proc = subprocess.Popen(
['espeak', '-s', '160', '"{0}"'.format(words)])
if block:
proc.wait()
except OSError as e:
logger.error(
'Unable to speak. Make sure "espeak" is installed locally.\n%s'
% str(e))
else:
self.soundhandle.say(words)
rospy.sleep(1)