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, 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 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
