def goto_position(self, position_for_motors, duration, wait=False): for motor_name, position in position_for_motors.iteritems(): m = getattr(self, motor_name) m.goto_position(position, duration) if wait: time.sleep(duration)
def goto_position(self, position, duration, control=None, wait=False): """ Automatically sets the goal position and the moving speed to reach the desired position within the duration. """ if control is None: control = self.goto_behavior if control == 'minjerk': goto_min_jerk = GotoMinJerk(self, position, duration) goto_min_jerk.start() if wait: goto_min_jerk.wait_to_stop() elif control == 'dummy': dp = abs(self.present_position - position) speed = (dp / float(duration)) if duration > 0 else 0 self.moving_speed = speed self.goal_position = position if wait: time.sleep(duration) elif control == 'linear': goto_linear = GotoLinear(self, position, duration) goto_linear.start() if wait: goto_linear.wait_to_stop()
def goto_position(self, position, duration, wait=False): """ Automatically sets the goal position and the moving speed to reach the desired position within the duration. """ dp = abs(self.present_position - position) speed = (dp / float(duration)) if duration > 0 else numpy.inf self.moving_speed = speed self.goal_position = position if wait: time.sleep(duration)
def goto_position(self, position_for_motors, duration, wait=False): """ Moves a subset of the motors to a position within a specific duration. :param dict position_for_motors: which motors you want to move {motor_name: pos, motor_name: pos,...} :param float duration: duration of the move :param bool wait: whether or not to wait for the end of the move .. note::In case of dynamixel motors, the speed is automatically adjusted so the goal position is reached after the chosen duration. """ for motor_name, position in position_for_motors.iteritems(): m = getattr(self, motor_name) m.goto_position(position, duration) if wait: time.sleep(duration)
def _open(self, port, baudrate, timeout, max_recursion=500): # Tries to connect to port until it succeeds to ping any motor on the bus. # This is used to circumvent a bug with the driver for the USB2AX on Mac. # Warning: If no motor is connected on the bus, this will run forever!!! import platform # import time import pypot.utils.pypot_time as time for i in range(max_recursion): self._known_models.clear() self._known_mode.clear() with self._serial_lock: self.close(_force_lock=True) if port in self.__used_ports: raise DxlError( 'Another instance of pypot is using the port {}. You should restart your Python kernel to pass through this issue.' .format(port)) # Dirty walkaround to fix a strange bug. # Observed with the USB2AX on Linux with pyserial 2.7 # We have to first open/close the port in order to make it work # at 1Mbauds if platform.system() == 'Linux' and self._sync_read: self._serial = serial.Serial(port, 9600) self._serial.close() self._serial = serial.Serial(port, baudrate, timeout=timeout, write_timeout=timeout) self.__used_ports.add(port) if (platform.system() == 'Darwin' and self._protocol.name == 'v1' and self._sync_read): if not self.ping(self._protocol.DxlBroadcast): self.close() continue else: time.sleep(self.timeout) self.flush() break else: raise DxlError('could not connect to the port {}'.format( self.port))
def _open(self, port, baudrate, timeout, max_recursion=500): # Tries to connect to port until it succeeds to ping any motor on the bus. # This is used to circumvent a bug with the driver for the USB2AX on Mac. # Warning: If no motor is connected on the bus, this will run forever!!! import platform # import time import pypot.utils.pypot_time as time for i in range(max_recursion): self._known_models.clear() self._known_mode.clear() with self._serial_lock: self.close(_force_lock=True) if port in self.__used_ports: raise DxlError('Another instance of pypot use the port {}. You should restart your Python kernel to pass through this issue.'.format(port)) # Dirty walkaround to fix a strange bug. # Observed with the USB2AX on Linux with pyserial 2.7 # We have to first open/close the port in order to make it work # at 1Mbauds if platform.system() == 'Linux' and self._sync_read: self._serial = serial.Serial(port, 9600) self._serial.close() self._serial = serial.Serial(port, baudrate, timeout=timeout) self.__used_ports.add(port) if (platform.system() == 'Darwin' and self._protocol.name == 'v1' and self._sync_read): if not self.ping(self._protocol.DxlBroadcast): self.close() continue else: time.sleep(self.timeout) self.flush() break else: raise DxlError('could not connect to the port {}'.format(self.port))
def goto_position(self, position, duration, control=None, wait=False): """ Reach the desired position within the specified duration""" #make sure we don't exceed the max playtime #this is done by adjusting the duration #TODO: improve by splitting the move accross several update cycles instead? duration = min(duration, hkx_max_playtime) #also keep track of the implied moving speed #if the implied speed is 0, we do NOT transform it to the maximum speed self.moving_speed = abs(position - self.present_position) / (duration) if control is None: control = self.goto_behavior #TODO: investigate minjerk through the min PWM register? if control == 'minjerk': pass elif control == 'dummy': dp = abs(self.present_position - position) speed = (dp / float(duration)) if duration > 0 else numpy.inf self.moving_speed = speed self.goal_position = position if wait: time.sleep(duration)
def from_vrep(config, vrep_host, vrep_port, vrep_scene, tracked_objects=[], tracked_collisions=[]): """ Create a robot from a V-REP instance. :param dict config: robot configuration dictionary :param str vrep_host: host of the V-REP server :param int vrep_port: port of the V-REP server :param str vrep_scene: path to the V-REP scene to load and start :param list tracked_objects: list of V-REP dummy object to track :param list tracked_collisions: list of V-REP collision to track This function tries to connect to a V-REP instance and expects to find motors with names corresponding as the ones found in the config. .. note:: The :class:`~pypot.robot.robot.Robot` returned will also provide a convenience reset_simulation method which resets the simulation and the robot position to its intial stance. .. note:: Using the same configuration, you should be able to switch from a real to a simulated robot just by switching from :func:`~pypot.robot.config.from_config` to :func:`~pypot.vrep.from_vrep`. For instance:: import json with open('my_config.json') as f: config = json.load(f) from pypot.robot import from_config from pypot.vrep import from_vrep real_robot = from_config(config) simulated_robot = from_vrep(config, '127.0.0.1', 19997, 'poppy.ttt') """ vrep_io = VrepIO(vrep_host, vrep_port) # The URDF uses the offset as the 0 for the motors equivalent # so we set all the offsets to 0 config = dict(config) for m in config['motors'].itervalues(): m['offset'] = 0.0 motors = [motor_from_confignode(config, name) for name in config['motors'].keys()] vc = VrepController(vrep_io, vrep_scene, motors) vc._init_vrep_streaming() sensor_controllers = [] if tracked_objects: sensors = [ObjectTracker(name) for name in tracked_objects] vot = VrepObjectTracker(vrep_io, sensors) sensor_controllers.append(vot) if tracked_collisions: sensors = [VrepCollisionDetector(name) for name in tracked_collisions] vct = VrepCollisionTracker(vrep_io, sensors) sensor_controllers.append(vct) robot = Robot(motor_controllers=[vc], sensor_controllers=sensor_controllers) init_pos = {m: m.goal_position for m in robot.motors} make_alias(config, robot) def reset(robot): for m, p in init_pos.iteritems(): m.goal_position = p if tracked_collisions: vct.stop() vrep_io.restart_simulation() if tracked_collisions: vct.start() robot.reset_simulation = lambda: reset(robot) def current_simulation_time(robot): return robot._controllers[0].io.get_simulation_current_time() Robot.current_simulation_time = property(lambda robot: current_simulation_time(robot)) res, tt = vrep.simxGetFloatSignal(robot._controllers[0].io.client_id, 'CurrentTime', vrep.simx_opmode_streaming) pypot_time.sleep(.1) vreptime = vrep_time(robot) pypot_time.time = vreptime.get_time # pypot_time.sleep = vreptime.sleep return robot