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) """