def AttachController(self, name, args, dof_indices, affine_dofs, simulated):
"""
Create and attach a controller to a subset of this robot's DOFs. If
simulated is False, a controller is created using 'args' and is attached
to the multicontroller. In simulation mode an IdealController is
created instead. Regardless of the simulation mode, the multicontroller
must be finalized before use.
@param name user-readable name used to identify this controller
@param args real controller arguments
@param dof_indices controlled joint DOFs
@param affine_dofs controleld affine DOFs
@param simulated simulation mode
@returns created controller
"""
if simulated:
args = 'IdealController'
delegate_controller = openravepy.RaveCreateController(self.GetEnv(), args)
if delegate_controller is None:
type_name = args.split()[0]
message = 'Creating controller {0:s} of type {1:s} failed.'.format(name, type_name)
raise openravepy.openrave_exception(message)
self.multicontroller.AttachController(delegate_controller, dof_indices, affine_dofs)
return delegate_controller
def AttachController(self, name, args, dof_indices, affine_dofs,
simulated):
"""
Create and attach a controller to a subset of this robot's DOFs. If
simulated is False, a controller is created using 'args' and is attached
to the multicontroller. In simulation mode an IdealController is
created instead. Regardless of the simulation mode, the multicontroller
must be finalized before use.
@param name user-readable name used to identify this controller
@param args real controller arguments
@param dof_indices controlled joint DOFs
@param affine_dofs controleld affine DOFs
@param simulated simulation mode
@returns created controller
"""
if simulated:
args = 'IdealController'
delegate_controller = openravepy.RaveCreateController(
self.GetEnv(), args)
if delegate_controller is None:
type_name = args.split()[0]
message = 'Creating controller {0:s} of type {1:s} failed.'.format(
name, type_name)
raise openravepy.openrave_exception(message)
self.multicontroller.AttachController(delegate_controller, dof_indices,
affine_dofs)
return delegate_controller
def LookAt(self, target, **kw_args):
"""Look at a point in the world frame.
Create and, optionally, execute a two waypoint trajectory that starts
at the current configuration and moves to an IK solution that is
looking at \a target.
@param target point in the world frame.
@param **kw_args keyword arguments passed to \ref MoveTo
@return pantilt trajectory
"""
dof_values = self.FindIK(target)
if dof_values is not None:
return self.MoveTo(dof_values, **kw_args)
else:
raise openravepy.openrave_exception('There is no IK solution available.')
def SetVelocity(self, q_dot):
with self.manip.GetRobot().GetEnv():
q_dot_limits = self.manip.GetRobot().GetDOFVelocityLimits(self.indices)
if not (numpy.abs(q_dot) <= q_dot_limits).all():
raise openravepy.openrave_exception('Desired velocity exceeds limits.')
with self.mutex:
if (q_dot != numpy.zeros(self.num_dofs)).any():
self.q_dot = numpy.array(q_dot, dtype='float')
self.watchdog = time.time()
self.running = True
else:
self.running = False
def LookAt(self, target, **kw_args):
"""Look at a point in the world frame.
Create and, optionally, execute a two waypoint trajectory that starts
at the current configuration and moves to an IK solution that is
looking at \a target.
@param target point in the world frame.
@param **kw_args keyword arguments passed to \ref MoveTo
@return pantilt trajectory
"""
dof_values = self.FindIK(target)
if dof_values is not None:
return self.MoveTo(dof_values, **kw_args)
else:
raise openravepy.openrave_exception(
'There is no IK solution available.')
def SetVelocity(self, q_dot):
with self.manip.GetRobot().GetEnv():
q_dot_limits = self.manip.GetRobot().GetDOFVelocityLimits(
self.indices)
if not (numpy.abs(q_dot) <= q_dot_limits).all():
raise openravepy.openrave_exception(
'Desired velocity exceeds limits.')
with self.mutex:
if (q_dot != numpy.zeros(self.num_dofs)).any():
self.q_dot = numpy.array(q_dot, dtype='float')
self.watchdog = time.time()
self.running = True
else:
self.running = False
parser.add_argument("-i", "--interactive", action="store_true", help="display the model in an OpenRAVE viewer")
parser.add_argument("-c", "--config", type=str, action="store", help="config file for urdf generation")
args = parser.parse_args()
# Load the plugin.
env = openravepy.Environment()
plugin = openravepy.RaveCreateModule(env, "urdf")
if plugin is None:
parser.error("Unable to load urdf plugin.")
sys.exit(1)
# Generate the KinBody XML.
try:
kinbody_xml = plugin.SendCommand("load {0:s} {1:s}".format(args.urdf_path, args.srdf_path))
if kinbody_xml is None:
raise openravepy.openrave_exception("An unknown error has occurred.")
except openravepy.openrave_exception, e:
parser.error("Failed generating KinBody: {0:s}".format(e.message))
sys.exit(1)
# env.SetViewer('qtcoin')
body = env.GetBodies()[0]
handles = list()
"""
for link in body.GetLinks():
pose = link.GetTransform()
handle = openravepy.misc.DrawAxes(env, pose, 0.2, 2)
handles.append(handle)
"""
def initialize(attach_viewer=False, sim=True, user_id='human', env=None):
"""Initialize the Human Robot"""
prpy.logger.initialize_logging()
if not env:
env = Environment()
#Setup Manipulators
with env:
robot = env.ReadKinBodyXMLFile('robots/man1.dae')
robot.SetName(user_id) #needed in order to have different humans in the same env
env.AddKinBody(robot)
robot.left_arm = robot.GetManipulator('leftarm')
robot.left_arm.hand = robot.left_arm.GetEndEffector()
robot.left_hand = robot.left_arm.hand
robot.right_arm = robot.GetManipulator('rightarm')
robot.right_arm.hand = robot.right_arm.GetEndEffector()
robot.right_hand = robot.right_arm.hand
bind_subclass(robot, Robot, robot_name=user_id)
bind_subclass(robot.left_arm, Manipulator)
bind_subclass(robot.right_arm, Manipulator)
bind_subclass(robot.left_arm.hand, HumanHand, manipulator=robot.left_arm, sim=True)
bind_subclass(robot.right_arm.hand, HumanHand, manipulator=robot.right_arm, sim=True)
#Setup Controller
args = 'IdealController'
affine_dofs=0
delegate_controllerr = RaveCreateController(env, args)
if delegate_controllerr is None:
message = 'Creating controller {0:s} of type {1:s} failed.'.format(robot.right_arm.GetName(), args)
raise openrave_exception(message)
robot.multicontroller.AttachController(delegate_controllerr, robot.right_arm.GetArmIndices(), affine_dofs)
delegate_controllerl = RaveCreateController(env, args)
if delegate_controllerl is None:
message = 'Creating controller {0:s} of type {1:s} failed.'.format(robot.left_arm.GetName(), args)
raise openrave_exception(message)
robot.multicontroller.AttachController(delegate_controllerl, robot.left_arm.GetArmIndices(), affine_dofs)
#Setup IK
ikmodel_left = InverseKinematicsModel(
robot,
iktype=IkParameterizationType.Transform6D,
manip=robot.left_arm)
if not ikmodel_left.load():
ikmodel_left.autogenerate()
ikmodel_right = InverseKinematicsModel(
robot,
iktype=IkParameterizationType.Transform6D,
manip=robot.right_arm)
if not ikmodel_right.load():
ikmodel_right.autogenerate()
#Setup planning pipeline
robot.planner = Sequence(
SnapPlanner(),
VectorFieldPlanner(),
CBiRRTPlanner())
robot.simplifier = None
robot.retimer = HauserParabolicSmoother() # hack
robot.smoother = HauserParabolicSmoother()
robot.actions = prpy.action.ActionLibrary()
#Setup viewer. Default to load rviz
if env.GetViewer() is None:
if attach_viewer == True:
attach_viewer = 'rviz'
env.SetViewer(attach_viewer)
#Fallback on qtcoin if rviz couldnt load
if env.GetViewer is None:
logger.warning('Loading the Rviz viewer failed. Falling back on qtcoin')
attach_viewer = 'qtcoin'
if attach_viewer and env.GetViewer() is None:
env.SetViewer(attach_viewer)
if env.GetViewer() is None:
raise Exception('Failed creating viewer of type "{0:s}"'.format(
attach_viewer))
#Remove ROS Logging since loading Rviz might have added it
prpy.logger.remove_ros_logger()
#changing orientation
with env:
robotLocation = numpy.array([[ -1. , 0. , 0. , 0.],
[ 0. , 0. , 1. , 0.],
[ 0. , 1. , 0. , 0.],
[ 0. , 0. , 0. , 1. ]])
robot.SetTransform(robotLocation)
return env, robot
help='config file for urdf generation')
args = parser.parse_args()
# Load the plugin.
env = openravepy.Environment()
plugin = openravepy.RaveCreateModule(env, 'urdf')
if plugin is None:
parser.error('Unable to load urdf plugin.')
sys.exit(1)
# Generate the KinBody XML.
try:
kinbody_xml = plugin.SendCommand('load {0:s} {1:s}'.format(
args.urdf_path, args.srdf_path))
if kinbody_xml is None:
raise openravepy.openrave_exception('An unknown error has occurred.')
except openravepy.openrave_exception, e:
parser.error('Failed generating KinBody: {0:s}'.format(e.message))
sys.exit(1)
#env.SetViewer('qtcoin')
body = env.GetBodies()[0]
handles = list()
"""
for link in body.GetLinks():
pose = link.GetTransform()
handle = openravepy.misc.DrawAxes(env, pose, 0.2, 2)
handles.append(handle)
"""
