diff --git a/bliss/common/axis.py b/bliss/common/axis.py index 009cf93abf8f71dd9772d38b7fee1cbb89aab688..409efd84d00706b7045fec5de5ad2d2c8129a5f2 100644 --- a/bliss/common/axis.py +++ b/bliss/common/axis.py @@ -37,6 +37,7 @@ from bliss.common.utils import Null, with_custom_members from bliss.config.static import get_config from bliss.common.encoder import Encoder from bliss.common.hook import MotionHook +from bliss.physics.trajectory import LinearTrajectory import gevent import re import math @@ -113,52 +114,57 @@ class Motion(object): return self.__type -class MotionEstimation(object): - """ - Estimate motion time and displacement based on current axis position - and configuration +class Trajectory(object): + """ Trajectory information + + Represents a specific trajectory motion. + """ + def __init__(self, axis, pvt): + """ + Args: + axis -- axis to which this motion corresponds to + pvt -- numpy array with three fields ('position','velocity','time') + """ + self.__axis = axis + self.__pvt = pvt - def __init__(self, axis, target_pos, initial_pos=None): - self.axis = axis - ipos = axis.position() if initial_pos is None else initial_pos - self.ipos = ipos - fpos = target_pos - delta = fpos - ipos - do_backlash = cmp(delta, 0) != cmp(axis.backlash, 0) - if do_backlash: - delta -= axis.backlash - fpos -= axis.backlash - self.fpos = fpos - self.displacement = displacement = abs(delta) - try: - self.vel = vel = axis.velocity() - self.accel = accel = axis.acceleration() - except NotImplementedError: - self.vel = float('+inf') - self.accel = float('+inf') - self.duration = 0 - return + @property + def axis(self): + return self.__axis - full_accel_time = vel / accel - full_accel_dplmnt = 0.5*accel * full_accel_time**2 - - full_dplmnt_non_const_vel = 2 * full_accel_dplmnt - reaches_max_velocity = displacement > full_dplmnt_non_const_vel - if reaches_max_velocity: - max_vel = vel - accel_time = full_accel_time - accel_dplmnt = full_accel_dplmnt - dplmnt_non_const_vel = full_dplmnt_non_const_vel - max_vel_dplmnt = displacement - dplmnt_non_const_vel - max_vel_time = max_vel_dplmnt / vel - self.duration = max_vel_time + 2*accel_time - else: - self.duration = math.sqrt(2*displacement/accel) + @property + def pvt(self): + return self.__pvt + + +def estimate_duration(axis, target_pos, initial_pos=None): + """ + Estimate motion time based on current axis position + and configuration + """ + ipos = axis.position() if initial_pos is None else initial_pos + fpos = target_pos + delta = fpos - ipos + do_backlash = cmp(delta, 0) != cmp(axis.backlash, 0) + if do_backlash: + delta -= axis.backlash + fpos -= axis.backlash + + try: + acc = axis.acceleration() + vel = axis.velocity() + except NotImplementedError: + # calc axes do not implement acceleration and velocity by default + return 0 + + linear_trajectory = LinearTrajectory(ipos, fpos, vel, acc) + duration = linear_trajectory.duration + if do_backlash: + backlash_estimation = estimate_duration(axis, target_pos, fpos) + duration += backlash_estimation + return duration - if do_backlash: - backlash_estimation = MotionEstimation(axis, target_pos, self.fpos) - self.duration += backlash_estimation.duration def lazy_init(func): @functools.wraps(func) @@ -618,14 +624,20 @@ class Axis(object): tuple: axis software limits (user units) """ if from_config: - ll = self.config.get("low_limit", float, None) - hl = self.config.get("high_limit", float, None) + ll = self.config.get("low_limit", float, float('-inf')) + hl = self.config.get("high_limit", float, float('+inf')) return map(self.dial2user, (ll, hl)) if not isinstance(low_limit, Null): self.settings.set("low_limit", low_limit) if not isinstance(high_limit, Null): self.settings.set("high_limit", high_limit) - return self.settings.get('low_limit'), self.settings.get('high_limit') + low_limit = self.settings.get('low_limit') + if low_limit is None: + low_limit = float('-inf') + high_limit = self.settings.get('high_limit') + if high_limit is None: + high_limit = float('+inf') + return low_limit, high_limit def _update_settings(self, state): self.settings.set("state", state) @@ -773,29 +785,21 @@ class Axis(object): # check software limits user_low_limit, user_high_limit = self.limits() - if user_low_limit is not None: - low_limit = self.user2dial(user_low_limit) * self.steps_per_unit - else: - low_limit = None - if user_high_limit is not None: - high_limit = self.user2dial(user_high_limit) * self.steps_per_unit - else: - high_limit = None - if high_limit is not None and high_limit < low_limit: + low_limit = self.user2dial(user_low_limit) * self.steps_per_unit + high_limit = self.user2dial(user_high_limit) * self.steps_per_unit + if high_limit < low_limit: high_limit, low_limit = low_limit, high_limit user_high_limit, user_low_limit = user_low_limit, user_high_limit backlash_str = " (with %f backlash)" % self.backlash if backlash else "" - if user_low_limit is not None: - if target_pos < low_limit: - raise ValueError( - "%s: move to `%f'%s would go below low limit (%f)" % - (self.name, user_target_pos, backlash_str, user_low_limit)) - if user_high_limit is not None: - if target_pos > high_limit: - raise ValueError( - "%s: move to `%f' %s would go beyond high limit (%f)" % - (self.name, user_target_pos, backlash_str, user_high_limit)) + if target_pos < low_limit: + raise ValueError( + "%s: move to `%f'%s would go below low limit (%f)" % + (self.name, user_target_pos, backlash_str, user_low_limit)) + if target_pos > high_limit: + raise ValueError( + "%s: move to `%f' %s would go beyond high limit (%f)" % + (self.name, user_target_pos, backlash_str, user_high_limit)) motion = Motion(self, target_pos, delta) motion.backlash = backlash diff --git a/bliss/common/motor_group.py b/bliss/common/motor_group.py index 5998702c56d11b7f7e3f08da54d9bb1ce4e1a34f..f22098956f9dfc54f9126d3241afe16320324090 100644 --- a/bliss/common/motor_group.py +++ b/bliss/common/motor_group.py @@ -7,8 +7,10 @@ import gevent import itertools +import numpy from bliss.common.task_utils import * from bliss.common.axis import Axis, AxisState, DEFAULT_POLLING_TIME +from bliss.common.axis import Trajectory from bliss.common import event from bliss.common.utils import grouped @@ -165,11 +167,13 @@ class _Group(object): self.__move_done.set() event.send(self, "move_done", True) - def move(self, *args, **kwargs): + def _check_ready(self): initial_state = self.state() if initial_state != "READY": raise RuntimeError("all motors are not ready") + def move(self, *args, **kwargs): + self._check_ready() self._reset_motions_dict() self.__move_task = None @@ -197,7 +201,10 @@ class _Group(object): motion) all_motions = self._start_motion(self._motions_dict) - self.__move_done.clear() + self._handle_motions(all_motions, wait, polling_time) + + def _handle_motions(self, all_motions, wait, polling_time): + self.__move_done.clear() self.__move_task = self._handle_move(all_motions, polling_time, wait=False) self.__move_task._motions = all_motions self.__move_task.link(self._set_move_done) @@ -216,3 +223,165 @@ class _Group(object): except gevent.GreenletExit: pass + +def TrajectoryGroup(*trajectories, **keys): + """ + Create an helper for a trajectory movement + Keys: + calc_axis -- calc axis link which has created this trajectory + """ + calc_axis = keys.pop('calc_axis', None) + traj = _TrajectoryGroup(calc_axis=calc_axis) + traj.trajectories = trajectories + return traj + +class _TrajectoryGroup(object): + """ + Group for motor trajectory + """ + def __init__(self, calc_axis=None): + self.__trajectories = None + self.__trajectories_dialunit = None + self.__group = None + self.__calc_axis = calc_axis + self.__disabled_axes = set() + + @property + def trajectories(self): + """ + Get/Set trajectories for this movement + """ + return self.__trajectories + @trajectories.setter + def trajectories(self, trajectories): + self.__trajectories = trajectories + self.__trajectories_dialunit = None + self.__group = Group(*self.axes) + self.__group.stop = self.stop + + @property + def axes(self): + """ + Axes for this motion + """ + return [t.axis for t in self.__trajectories] + + @property + def disabled_axes(self): + """ + Axes which are disabled for the next motion + """ + return self.__disabled_axes + + def disable_axis(self, axis): + """ + Disable an axis for the next motion + """ + self.__disabled_axes.add(axis) + + def enable_axis(self, axis): + """ + Enable an axis for the next motion + """ + try: + self.__disabled_axes.remove(axis) + except KeyError: # was already enable + pass # should we raise? + + @property + def calc_axis(self): + """ + calculation axis if any + """ + return self.__calc_axis + + def prepare(self): + """ + prepare/load trajectories in controllers + """ + if self.__trajectories_dialunit is None: + trajectories = list() + for trajectory in self.__trajectories: + user_pos = trajectory.pvt['position'] + user_velocity = trajectory.pvt['velocity'] + pvt = numpy.copy(trajectory.pvt) + pvt['position'] = trajectory.axis.user2dial(user_pos) * \ + trajectory.axis.steps_per_unit + pvt['velocity'] *= trajectory.axis.steps_per_unit + trajectories.append(Trajectory(trajectory.axis, pvt)) + self.__trajectories_dialunit = trajectories + self._exec_func_on_controller('prepare_trajectory') + + def move_to_start(self, wait=True, polling_time=DEFAULT_POLLING_TIME): + """ + Move all enabled motors to the first point of the trajectory + """ + self.__group._check_ready() + all_motions = list() + for trajectory in self.__trajectories: + pvt = trajectory.pvt + final_pos = pvt['position'][0] + motion = trajectory.axis.prepare_move(final_pos) + if not motion: + # already at final pos + continue + #no backlash to go to the first position + #otherwise it may break next trajectory motion (move_to_end) + motion.backlash = 0 + all_motions.append(motion) + + self._exec_func_on_controller('move_to_trajectory') + self.__group._handle_motions(all_motions, wait, polling_time) + + def move_to_end(self, wait=True, polling_time=DEFAULT_POLLING_TIME): + """ + Move all enabled motors to the last point of the trajectory + """ + self.__group._check_ready() + all_motions = list() + for trajectory in self.__trajectories: + pvt = trajectory.pvt + final_pos = pvt['position'][-1] + motion = trajectory.axis.prepare_move(final_pos) + if not motion: + continue + all_motions.append(motion) + + self._exec_func_on_controller('start_trajectory') + self.__group._handle_motions(all_motions, wait, polling_time) + + def stop(self, wait=True): + """ + Stop the motion an all motors + """ + self._exec_func_on_controller('stop_trajectory') + if wait: + self.__group.wait_move() + + def state(self): + """ + Get the trajectory group status + """ + return self.__group.state() + + def wait_move(self): + """ + Wait the end of motion + """ + self.__group.wait_move() + + def _exec_func_on_controller(self, funct_name): + tasks = list() + for ctrl, trajectories in self._group_per_controller().iteritems(): + funct = getattr(ctrl, funct_name) + tasks.append(gevent.spawn(funct, *trajectories)) + gevent.joinall(tasks, raise_error=True) + + def _group_per_controller(self): + controller_trajectories = dict() + for traj in self.__trajectories_dialunit: + if traj.axis in self.__disabled_axes: + continue + tlist = controller_trajectories.setdefault(traj.axis.controller, []) + tlist.append(traj) + return controller_trajectories diff --git a/bliss/common/scans.py b/bliss/common/scans.py index 38bc5fe77ec9e545dfa0e079110c91b49572304c..917033bf79d1c751917750de0e4267ca59e19a39 100644 --- a/bliss/common/scans.py +++ b/bliss/common/scans.py @@ -22,7 +22,7 @@ import numpy import gevent from bliss import setup_globals -from bliss.common.axis import MotionEstimation +from bliss.common.axis import estimate_duration from bliss.common.temperature import Input, Output, TempControllerCounter from bliss.controllers.lima import Lima from bliss.controllers.ct2.client import CT2 @@ -286,8 +286,8 @@ def ascan(motor, start, stop, npoints, count_time, *counters, **kwargs): # estimate scan time step_size = abs(stop - start) / float(npoints) - i_motion_t = MotionEstimation(motor, start).duration - n_motion_t = MotionEstimation(motor, start, start + step_size).duration + i_motion_t = estimate_duration(motor, start) + n_motion_t = estimate_duration(motor, start, start + step_size) total_motion_t = i_motion_t + npoints * n_motion_t total_count_t = npoints * count_time estimation = {'total_motion_time': total_motion_t, @@ -391,14 +391,14 @@ def mesh(motor1, start1, stop1, npoints1, motor2, start2, stop2, npoints2, count # estimate scan time step_size1 = abs(stop1 - start1) / float(npoints1) - i_motion_t1 = MotionEstimation(motor1, start1).duration - n_motion_t1 = MotionEstimation(motor1, start1, start1 + step_size1).duration + i_motion_t1 = estimate_duration(motor1, start1) + n_motion_t1 = estimate_duration(motor1, start1, start1 + step_size1) total_motion_t1 = npoints1 *npoints2 * n_motion_t1 step_size2 = abs(stop2 - start2) / float(npoints2) - i_motion_t2 = MotionEstimation(motor2, start2).duration - n_motion_t2 = max(MotionEstimation(motor2, start2, start2 + step_size2).duration, - MotionEstimation(motor1, end1, start1).duration) + i_motion_t2 = estimate_duration(motor2, start2) + n_motion_t2 = max(estimate_duration(motor2, start2, start2 + step_size2), + estimate_duration(motor1, end1, start1)) total_motion_t2 = npoints2 * n_motion_t2 imotion_t = max(i_motion_t1, i_motion_t2) @@ -488,12 +488,12 @@ def a2scan(motor1, start1, stop1, motor2, start2, stop2, npoints, count_time, # estimate scan time step_size1 = abs(stop1 - start1) / float(npoints) - i_motion1_t = MotionEstimation(motor1, start1).duration - n_motion1_t = MotionEstimation(motor1, start1, start1 + step_size1).duration + i_motion1_t = estimate_duration(motor1, start1) + n_motion1_t = estimate_duration(motor1, start1, start1 + step_size1) step_size2 = abs(stop2 - start2) / float(npoints) - i_motion2_t = MotionEstimation(motor2, start2).duration - n_motion2_t = MotionEstimation(motor2, start2, start2 + step_size2).duration + i_motion2_t = estimate_duration(motor2, start2) + n_motion2_t = estimate_duration(motor2, start2, start2 + step_size2) i_motion_t = max(i_motion1_t, i_motion2_t) n_motion_t = max(n_motion1_t, n_motion2_t) diff --git a/bliss/config/conductor/connection.py b/bliss/config/conductor/connection.py index 933bdf4cee2f544123458d086bd98b6b63f3b984..49d404b7ac81fb630a6543672d27f07b476f27ad 100644 --- a/bliss/config/conductor/connection.py +++ b/bliss/config/conductor/connection.py @@ -280,7 +280,7 @@ class Connection(object): if isinstance(value,RuntimeError): raise value else: - return value.decode("utf-8") + return value @check_connect def get_config_db_tree(self, base_path='', timeout=1.): diff --git a/bliss/config/conductor/protocol.py b/bliss/config/conductor/protocol.py index 35751948a3fa02bf842bdc30f2e2989812d63d1f..0677f449c69456a2fb6ebfd999110276e07a825e 100644 --- a/bliss/config/conductor/protocol.py +++ b/bliss/config/conductor/protocol.py @@ -7,7 +7,6 @@ import struct -DEFAULT_UDP_CLIENT_PORT = 8021 DEFAULT_UDP_SERVER_PORT = 8020 HEADER_SIZE = struct.calcsize('= trajectory_resolution + >= trajectory_minimum_resolution): + if trajectory_resolution > trajectory_minimum_resolution: + used_resolution = trajectory_minimum_resolution + else: + used_resolution = trajectory_maximum_resolution + elif trajectory_minimum_resolution is not None: + if trajectory_resolution > trajectory_minimum_resolution: + used_resolution = trajectory_minimum_resolution + elif trajectory_maximum_resolution is not None: + if trajectory_resolution < trajectory_maximum_resolution: + used_resolution = trajectory_maximum_resolution + + if used_resolution is not None: + new_nb_points = int(round(total_distance / used_resolution)) + new_time_point = float(time_per_point * nb_points) / new_nb_points + nb_points = new_nb_points + time_per_point = new_time_point + + calc_positions = numpy.linspace(start_point, end_point, nb_points) + positions = {self._axis_tag(calc_axis) : calc_positions} + #other virtual axis stays at the same position + for caxis in self.pseudos: + if caxis is calc_axis: + continue + cpos = numpy.zeros(len(calc_positions), dtype=numpy.float) + cpos[:] = caxis.position() + positions[self._axis_tag(caxis)] = cpos + + time = numpy.linspace(0., nb_points * time_per_point, nb_points) + real_positions = self.calc_to_real(positions) + final_real_axes_position = dict() + self._get_real_position(real_axes, real_positions, + final_real_axes_position) + + + pt = trajectory.PointTrajectory() + spline_nb_points = 0 if interpolation_factor == 1 \ + else len(time) * interpolation_factor + pt.build(time, {axis.name:position + for axis, position in final_real_axes_position.iteritems()}, + spline_nb_points=spline_nb_points) + #check velocity and acceleration + max_velocity = pt.max_velocity() + max_acceleration = pt.max_acceleration() + limits = pt.limits() + error_list = list() + start_stop_acceleration = dict() + for axis in final_real_axes_position: + vel = axis.velocity() + acc = axis.acceleration() + axis_limits = axis.limits() + traj_vel = max_velocity[axis.name] + traj_acc = max_acceleration[axis.name] + traj_limits = limits[axis.name] + if traj_acc > acc: + error_list.append("Axis %s reach %f acceleration on this trajectory," + "max acceleration is %f" % (axis.name, traj_acc, acc)) + if traj_vel > vel: + error_list.append("Axis %s reach %f velocity on this trajectory," + "max velocity is %f" % (axis.name, traj_vel, vel)) + for lm in traj_limits: + if not axis_limits[0] <= lm <= axis_limits[1]: + error_list.append("Axis %s go beyond limits (%f <= %f <= %f)" % + (axis.name, axis_limits[0], + traj_limits[0], axis_limits[1])) + + start_stop_acceleration[axis.name] = acc + + if error_list: + error_message = "Trajectory on calc axis **%s** can not be done.\n" % calc_axis.name + error_message += '\n'.join(error_list) + raise ValueError(error_message) + + pvt = pt.pvt(acceleration_start_end=start_stop_acceleration) + trajectories = [Trajectory(axis, pvt[axis.name]) \ + for axis in final_real_axes_position] + + return TrajectoryGroup(*trajectories, calc_axis=calc_axis) + + def _check_trajectory(self, axis): + if axis.controller.has_trajectory(): + return axis, [] + else: # check if axis is part of calccontroller + ctrl = axis.controller + if isinstance(ctrl, CalcController): + real_axes = list() + for real in ctrl.reals: + raxis, axes = self._check_trajectory(real) + real_axes.append((raxis, axes)) + return axis, real_axes + else: + raise ValueError("Controller for axis %s does not support " + "trajectories" % axis.name) + + def _get_real_position(self, real_axes, real_positions, + final_real_axes_position): + + local_real_positions = dict() + for axis, dep_real_axes in real_axes: + axis_position = real_positions.get(self._axis_tag(axis)) + if not dep_real_axes: + if axis_position is None: + raise RuntimeError("Could not get position " + "for axis %s" % axis.name) + else: + final_real_axes_position[axis] = axis_position + else: + ctrl = axis.controller + local_real_positions = {ctrl._axis_tag(axis) : axis_position} + for caxis in ctrl.pseudos: + axis_tag = ctrl._axis_tag(caxis) + if caxis is axis or \ + axis_tag in local_real_positions: + continue + cpos = numpy.zeros(len(axis_position), dtype=numpy.float) + cpos[:] = caxis.position() + local_real_positions[ctrl._axis_tag(caxis)] = cpos + + dep_real_position = ctrl.calc_to_real(local_real_positions) + ctrl._get_real_position(dep_real_axes, dep_real_position, + final_real_axes_position) diff --git a/bliss/controllers/motors/icepap/__init__.py b/bliss/controllers/motors/icepap/__init__.py index f273a79a8457ecbff359979c9913f88114aa60c9..8246b26e46d8a9eed72bb4364c5d4d3dec75822d 100644 --- a/bliss/controllers/motors/icepap/__init__.py +++ b/bliss/controllers/motors/icepap/__init__.py @@ -8,8 +8,10 @@ import re import time import gevent +import hashlib import functools from bliss.common.greenlet_utils import protect_from_kill +from bliss.config.channels import Cache from bliss.controllers.motor import Controller from bliss.common.axis import AxisState,Axis from bliss.common.utils import object_method @@ -18,6 +20,10 @@ import struct import numpy import sys +def _object_method_filter(obj): + if isinstance(obj, (LinkedAxis, TrajectoryAxis)): + return False + return True class Icepap(Controller): """ @@ -82,7 +88,9 @@ class Icepap(Controller): self._cnx.close() def initialize_axis(self,axis): - axis.address = axis.config.get("address",lambda x: x) + if not isinstance(axis, TrajectoryAxis): + axis.address = axis.config.get("address",lambda x: x) + axis._trajectory_cache = Cache(axis, "trajectory_cache") if hasattr(axis,'_init_software'): axis._init_software() @@ -111,11 +119,17 @@ class Icepap(Controller): self._power(axis,False) def _power(self,axis,power): + if isinstance(axis, TrajectoryAxis): + return + _ackcommand(self._cnx,"POWER %s %s" % ("ON" if power else "OFF",axis.address)) self._last_axis_power_time[axis] = time.time() def read_position(self,axis,cache=True): + if isinstance(axis, TrajectoryAxis): + return axis._read_position() + pos_cmd = "FPOS" if cache else "POS" return int(_command(self._cnx,"?%s %s" % (pos_cmd,axis.address))) @@ -129,15 +143,24 @@ class Icepap(Controller): return self.read_position(axis,cache=False) def read_velocity(self,axis): + if isinstance(axis, TrajectoryAxis): + return axis._get_velocity() + return float(_command(self._cnx,"?VELOCITY %s" % axis.address)) def set_velocity(self,axis,new_velocity): + if isinstance(axis, TrajectoryAxis): + return axis._set_velocity(new_velocity) + _ackcommand(self._cnx,"VELOCITY %s %f" % (axis.address,new_velocity)) return self.read_velocity(axis) def read_acceleration(self,axis): - acctime = float(_command(self._cnx,"?ACCTIME %s" % axis.address)) + if isinstance(axis, TrajectoryAxis): + acctime = axis._get_acceleration_time() + else: + acctime = float(_command(self._cnx,"?ACCTIME %s" % axis.address)) velocity = self.read_velocity(axis) return velocity/float(acctime) @@ -145,19 +168,25 @@ class Icepap(Controller): velocity = self.read_velocity(axis) new_acctime = velocity/new_acc + if isinstance(axis, TrajectoryAxis): + return axis._set_acceleration_time(new_acctime) + _ackcommand(self._cnx,"ACCTIME %s %f" % (axis.address,new_acctime)) return self.read_acceleration(axis) def state(self,axis): - last_power_time = self._last_axis_power_time.get(axis,0) - if time.time() - last_power_time < 1.: - status_cmd = "?STATUS" + if isinstance(axis, TrajectoryAxis): + status = axis._state() else: - self._last_axis_power_time.pop(axis,None) - status_cmd = "?FSTATUS" + last_power_time = self._last_axis_power_time.get(axis,0) + if time.time() - last_power_time < 1.: + status_cmd = "?STATUS" + else: + self._last_axis_power_time.pop(axis,None) + status_cmd = "?FSTATUS" - status = int(_command(self._cnx,"%s %s" % - (status_cmd,axis.address)),16) + status = int(_command(self._cnx,"%s %s" % + (status_cmd,axis.address)),16) status ^= 1<<23 #neg POWERON FLAG state = self._icestate.new() for mask,value in (((1<<9),"READY"), @@ -191,16 +220,17 @@ class Icepap(Controller): if status & (1<<13): try: warning = _command(self._cnx,"%d:?WARNING" % axis.address) - except TypeError: + except (TypeError, AttributeError): pass else: - warn_str = "warning condition: \n" + warning - status.create_state("WARNING",warn_str) - status.set("WARNING") + warn_str = "Axis %s warning condition: \n" % axis.name + warn_str += warning + state.create_state("WARNING",warn_str) + state.set("WARNING") try: alarm = _command(self._cnx,"%d:?ALARM" % axis.address) - except (RuntimeError,TypeError): + except (RuntimeError, TypeError, AttributeError): pass else: if alarm != "NO": @@ -233,7 +263,9 @@ class Icepap(Controller): pass def start_one(self,motion): - if isinstance(motion.axis,SlaveAxis): + if isinstance(motion.axis, TrajectoryAxis): + return motion.axis._start_one(motion) + elif isinstance(motion.axis,SlaveAxis): pre_cmd = "%d:DISPROT LINKED;" % motion.axis.address else: pre_cmd = None @@ -243,7 +275,7 @@ class Icepap(Controller): pre_cmd = pre_cmd) def start_all(self,*motions): - if motions > 1: + if len(motions) > 1: cmd = "MOVE GROUP " cmd += ' '.join(["%s %d" % (m.axis.address,m.target_pos) for m in motions]) _ackcommand(self._cnx,cmd) @@ -251,11 +283,17 @@ class Icepap(Controller): self.start_one(motions[0]) def stop(self,axis): - _command(self._cnx,"STOP %s" % axis.address) + if isinstance(axis, TrajectoryAxis): + return axis._stop() + else: + _command(self._cnx,"STOP %s" % axis.address) def stop_all(self,*motions): - for motion in motions: - self.stop(motion.axis) + if len(motions) > 1: + axes_addr = ' '.join('%s' % m.axis.address for m in motions) + _command(self._cnx,"STOP %s" % axes_addr) + else: + self.stop(motions[0].axis) def home_search(self,axis,switch): cmd = "HOME " + ("+1" if switch > 0 else "-1") @@ -354,16 +392,70 @@ class Icepap(Controller): linked[values[0]] = [int(x) for x in values[1:]] return linked - @object_method(types_info=("bool","bool")) + def has_trajectory(self): + return True + + def prepare_trajectory(self, *trajectories): + if not trajectories: + raise ValueError("no trajectory provided") + + update_cache = list() + data = numpy.array([],dtype=numpy.int8) + for traj in trajectories: + pvt = traj.pvt + axis = traj.axis + axis_data = _vdata_header(pvt['position'], axis, POSITION) + axis_data = numpy.append(axis_data, _vdata_header(pvt['time'], + axis, PARAMETER)) + axis_data = numpy.append(axis_data, _vdata_header(pvt['velocity'], + axis, SLOPE)) + h = hashlib.md5() + h.update(axis_data.tostring()) + digest = h.hexdigest() + if axis._trajectory_cache.value != digest: + data = numpy.append(data, axis_data) + update_cache.append((axis._trajectory_cache, digest)) + + if not data.size: # nothing to do + return + + _command(self._cnx, "#*PARDAT", data=data) + #update axis trajectory cache + for cache, value in update_cache: + cache.value = value + + axes_str = ' '.join(('%s' % traj.axis.address for traj in trajectories)) + _command(self._cnx,"#PARVEL 1 %s" % axes_str) + _command(self._cnx,"#PARACCT 0 {}".format(axes_str)) + + def move_to_trajectory(self, *trajectories): + axes_str = ' '.join(('%s' % traj.axis.address for traj in trajectories)) + #Doesn't work yet + #_command(self._cnx,"#MOVEP 0 GROUP %s" % axes_str) + _command(self._cnx,"#MOVEP 0 %s" % axes_str) + + def start_trajectory(self, *trajectories): + axes_str = ' '.join(('%s' % traj.axis.address for traj in trajectories)) + traj1 = trajectories[0] + endtime = traj1.pvt['time'][-1] + #Doesn't work yet + #_command(self._cnx,"#PMOVE %lf GROUP %s" % (endtime,axes_str)) + _command(self._cnx,"#PMOVE {} {}".format(endtime,axes_str)) + + def stop_trajectory(self, *trajectories): + axes_str = ' '.join(('%s' % traj.axis.address for traj in trajectories)) + _command(self._cnx,"STOP %s" % axes_str) + + @object_method(types_info=("bool","bool"), filter=_object_method_filter) def activate_closed_loop(self,axis,active): _command(self._cnx,"#%s:PCLOOP %s" % (axis.address,"ON" if active else "OFF")) return active - @object_method(types_info=("None","bool")) + @object_method(types_info=("None","bool"), filter=_object_method_filter) def is_closed_loop_activate(self,axis): return True if _command(self._cnx,"%s:?PCLOOP" % axis.address) == 'ON' else False - @object_method(types_info=("None","None")) + @object_method(types_info=("None","None"), filter=_object_method_filter) def reset_closed_loop(self,axis): measure_position = int(_command(self._cnx,"%s:?POS MEASURE" % axis.address)) self.set_position(axis,measure_position) @@ -371,11 +463,11 @@ class Icepap(Controller): self.set_on(axis) axis.sync_hard() - @object_method(types_info=("None","int")) + @object_method(types_info=("None","int"), filter=_object_method_filter) def temperature(self,axis): return int(_command(self._cnx,"%s:?MEAS T" % axis.address)) - @object_method(types_info=(("float","bool"),"None")) + @object_method(types_info=(("float","bool"),"None"), filter=_object_method_filter) def set_tracking_positions(self,axis,positions,cyclic = False): """ Send position to the controller which will be tracked. @@ -397,7 +489,7 @@ class Icepap(Controller): (address, "CYCLIC" if cyclic else "NOCYCLIC"), step_positions) - @object_method(types_info=("None",("float","bool"))) + @object_method(types_info=("None",("float","bool")), filter=_object_method_filter) def get_tracking_positions(self,axis): """ Get the tacking positions. @@ -426,7 +518,7 @@ class Icepap(Controller): positions = axis.dial2user(dial_positions) return positions,cyclic - @object_method(types_info=(("bool","str"),"None")) + @object_method(types_info=(("bool","str"),"None"), filter=_object_method_filter) def activate_tracking(self,axis,activate,mode = None): """ Activate/Deactivate the tracking position depending on @@ -453,7 +545,7 @@ class Icepap(Controller): _ackcommand(self._cnx, "%d:POS INPOS 0" % address) _ackcommand(self._cnx,"%d:LTRACK %s" % (address,mode)) - @object_method(types_info=("float", "None")) + @object_method(types_info=("float", "None"), filter=_object_method_filter) def blink(self, axis, second=3.): """ Blink axis driver @@ -474,6 +566,49 @@ class Icepap(Controller): self._cnx.close() _check_reply = re.compile("^[#?]|^[0-9]+:\?") +PARAMETER,POSITION,SLOPE = (0x1000, 0x2000, 0x4000) + +def _vdata_header(data,axis,vdata_type): + PARDATA_HEADER_FORMAT=' 0xFFFF: + raise ValueError("too many data values, max: 0xFFFF") + + dtype = numpydtype_2_dtype[data.dtype] + data_test = data.newbyteorder('<') + if data_test[0] != data[0]: # not good endianness + data = data.byteswap() + + header_size = struct.calcsize(PARDATA_HEADER_FORMAT) + full_size = header_size + len(data.tostring()) + aligned_full_size = (full_size + 3) & ~3 # alignment 32 bits + flags = vdata_type | axis.address + bin_header = struct.pack(PARDATA_HEADER_FORMAT, + 0xCAFE, # vdata signature + 0, # Version = 0 + header_size / 4, # Data offset in dwords + aligned_full_size / 4, # Full vector size in dwords + len(data), # number of values in the vector + dtype, # Data type + 0, # no compression + flags, # format + address + 0) # first data value for incremental coding + return numpy.fromstring(bin_header + data.tostring() + \ + '\0' * (aligned_full_size-full_size),dtype=numpy.int8) + @protect_from_kill def _command(cnx,cmd,data = None,pre_cmd = None): if data is not None: @@ -513,3 +648,4 @@ def _ackcommand(cnx,cmd,data = None,pre_cmd = None): from .shutter import Shutter from .switch import Switch from .linked import LinkedAxis, SlaveAxis +from .trajectory import TrajectoryAxis diff --git a/bliss/controllers/motors/icepap/switch.py b/bliss/controllers/motors/icepap/switch.py index 3a45d7ae746f92ed92217ea687b485f48d0b34b8..bfe458ca51d2094b2139a2ce048fd7a3d3ff6111 100644 --- a/bliss/controllers/motors/icepap/switch.py +++ b/bliss/controllers/motors/icepap/switch.py @@ -43,7 +43,10 @@ class Switch(BaseSwitch): for axis in self.__controller._axes.values(): # be sure that axis is initialized axis.position() - include_rack.add(axis.address // 10) + try: + include_rack.add(axis.address // 10) + except (AttributeError, TypeError): # LinkedAxis and TrajectoryAxis + continue else: include_rack = set(include_rack) @@ -56,7 +59,10 @@ class Switch(BaseSwitch): managed_rack = include_rack - exclude_rack self.__axes = weakref.WeakValueDictionary() for axis_name,axis in self.__controller._axes.iteritems(): - rack_id = axis.address // 10 + try: + rack_id = axis.address // 10 + except (AttributeError, TypeError): + continue if rack_id in managed_rack: self.__axes[axis_name.upper()] = axis diff --git a/bliss/controllers/motors/icepap/trajectory.py b/bliss/controllers/motors/icepap/trajectory.py new file mode 100644 index 0000000000000000000000000000000000000000..b0d40aa84dc9c31fa343ddef511f75959ac8d7b1 --- /dev/null +++ b/bliss/controllers/motors/icepap/trajectory.py @@ -0,0 +1,295 @@ +# -*- coding: utf-8 -*- +# +# This file is part of the bliss project +# +# Copyright (c) 2018 Beamline Control Unit, ESRF +# Distributed under the GNU LGPLv3. See LICENSE for more info. + +import functools +import hashlib +import numpy +import mock +from bliss.common.axis import Axis, lazy_init, DEFAULT_POLLING_TIME +from . import _command, _vdata_header, POSITION, PARAMETER + +def check_initialized(func): + @functools.wraps(func) + def func_wrapper(self, *args, **kwargs): + if self._axes is None: + raise RuntimeError("Axis ** %s ** not initialized, " + "hint: call set_positions" % self.name) + return func(self, *args, **kwargs) + return func_wrapper + +class TrajectoryAxis(Axis): + """ + Virtual Icepap axis with follow a trajectory defined by + a position table. + You need to load a trajectory table with method + **set_positions** before using the axis. + """ + SPLINE, LINEAR, CYCLIC = range(3) + + def __init__(self, name, controller, config): + Axis.__init__(self, name, controller, config) + self.settings.get = mock.MagicMock(return_value=None) + self.settings.set = mock.MagicMock(return_value=None) + + self._axes = None + self._parameter = None + self._positions = None + self._trajectory_mode = TrajectoryAxis.SPLINE + self._disabled_axes = set() + self._hash_cache = dict() + + self.auto_join_trajectory = config.get('auto_join_trajectory', 'True') + self._config_velocity = -1 # auto max vel on the trajectory + self._config_acceleration = -1 # auto max acceleration for motors involved + self._velocity = -1 + self._acceleration_time = -1 + + @property + def disabled_axes(self): + """ + Axes which motion are disabled. + """ + return self._disabled_axes + + def disable_axis(self, axis): + """ + Disable motion of a real axis. + """ + self._disabled_axes.add(axis) + + @property + def enabled_axes(self): + """ + Axes which motion are enabled. + """ + return set(self.real_axes) - self.disabled_axes + + def enable_axis(self, axis): + """ + Enable motion of a real axis. + """ + try: + self._disabled_axes.remove(axis) + except KeyError: + pass + @lazy_init + def set_positions(self, parameter, positions, + trajectory_mode=SPLINE): + """ + Set the real axes positions for this virtual motor. + + Args: + parameter: apse of all real motor positions + positions: a dictionnary with the key as the name of the + motor and with the value as the position of this motor. + trajectory_mode: default is SPLINE but could be CYCLIC or LINEAR + """ + axes = dict() + for name, axis in self.controller.axes.iteritems(): + if name in positions: + axes[axis.name] = axis + positions[axis.name] *= axis.steps_per_unit + if len(positions) > axes: + raise RuntimeError("Axis %s, real axes (%s) are not " + "managed in this controller" % + (self.name, ','.join(set(positions)-set(axes)))) + self._hash_cache = dict() + self._trajectory_mode = trajectory_mode + self._load_trajectories(axes, parameter, positions) + self._axes = axes + self._disabled_axes = set() + self._parameter = parameter + self._positions = positions + self._set_velocity(self._config_velocity) + self._set_acceleration_time(self._config_acceleration) + + def get_positions(self): + """ + Positions of all real axes + """ + return self._parameter, self._positions + + @property + @check_initialized + def real_motor_names(self): + """ + Return a list of real motor linked to this virtual axis + """ + return self._axes.keys() + + @property + @check_initialized + def real_axes(self): + """ + Return a list of real axis linked to this virtual axis + """ + return self._axes.values() + + @check_initialized + def movep(self, user_target_pos, wait=True, relative=False, + polling_time=DEFAULT_POLLING_TIME): + """ + movement to parameter value + """ + #check if trajectories are loaded + self._load_trajectories(self._axes, self._parameter, self._positions) + axes_str = ' '.join(('%s' % axis.address for axis in self.enabled_axes)) + motion = self.prepare_move(user_target_pos, relative) + + _command(self.controller._cnx, + "#MOVEP {} {}".format(motion.target_pos, axes_str)) + + motion_task = self._start_move_task(self._do_handle_move, motion, + polling_time, being_waited=wait) + motion_task._motions = [motion] + + if wait: + self.wait_move() + + def _init_software(self): + try: + self._config_velocity = self.config.get('velocity', float) + except KeyError: + self.config.set('velocity', -1) # maximum for a trajectory + + try: + self._config_acceleration = self.config.get('acceleration', float) + except KeyError: + # maximum accelaration for motor involved + self.config.set('acceleration', -1) + + def _load_trajectories(self, axes, parameter, positions): + data = numpy.array([], dtype=numpy.int8) + update_cache = list() + for mot_name, pos in positions.iteritems(): + axis = axes[mot_name] + if axis._trajectory_cache.value == self._hash_cache.get(mot_name,numpy.nan): + continue + + axis_data = _vdata_header(pos, axis, POSITION) + axis_data = numpy.append(axis_data, + _vdata_header(parameter, axis, PARAMETER)) + h = hashlib.md5() + h.update(axis_data.tostring()) + digest = h.hexdigest() + if axis._trajectory_cache.value != digest: + data = numpy.append(data, axis_data) + update_cache.append((axis, digest)) + else: + self._hash_cache[axis.name] = digest + + if not data.size: # nothing to do + return + t_mode = {TrajectoryAxis.LINEAR:'LINEAR', + TrajectoryAxis.SPLINE:'SPLINE', + TrajectoryAxis.CYCLIC:'CYCLIC'} + t_mode_str = t_mode.get(self._trajectory_mode) + + _command(self.controller._cnx, + "#*PARDAT {}".format(t_mode_str), data=data) + #update axis trajectory cache + for axis, value in update_cache: + axis._trajectory_cache.value = value + self._hash_cache[axis.name] = value + + @check_initialized + def _start_one(self, motion): + target_pos = motion.target_pos + #check if trajectories are loaded + self._load_trajectories(self._axes, self._parameter, self._positions) + axes_str = ' '.join(('%s' % axis.address for axis in self.enabled_axes)) + try: + _command(self.controller._cnx, + "#PMOVE {} {}".format(target_pos, axes_str)) + except RuntimeError: + if self.auto_join_trajectory: + _command(self.controller._cnx, + "#MOVEP {} {}".format(target_pos, axes_str)) + else: + raise + + def _stop(self): + """ + Stop all real axes + """ + axes_str = ' '.join(('%s' % axis.address for axis in self.enabled_axes)) + _command(self.controller._cnx, "STOP %s" % axes_str) + + def _set_velocity(self, velocity): + if self._axes: # trajectory is already loaded + self._load_trajectories(self._axes, self._parameter, + self._positions) + if velocity < 0: # get the max for this trajectory + max_velocity = None + max_acceleration = None + for axis in self.real_axes: + max_axis_vel = float(_command(self.controller._cnx, + "%d:?PARVEL max" % axis.address)) + max_axis_vel = min(axis.velocity() * axis.steps_per_unit, + max_axis_vel) + if max_velocity is None or max_axis_vel < max_velocity: + max_velocity = max_axis_vel + + velocity = max_velocity + axes_str = ' '.join(('%s' % axis.address for axis in self.real_axes)) + _command(self.controller._cnx,"#PARVEL {} {}".format(velocity, + axes_str)) + self._velocity = velocity + return velocity + + def _get_velocity(self): + return self._velocity + + def _set_acceleration_time(self, acceleration_time): + if self._axes: # trajectory is already loaded + self._load_trajectories(self._axes, self._parameter, + self._positions) + if acceleration_time < 0: # get the max for this trajectory + min_acceleration_time = None + for axis in self.real_axes: + axis_acceleration_time = axis.acctime() + if min_acceleration_time is None or \ + axis_acceleration_time > min_acceleration_time: + min_acceleration_time = axis_acceleration_time + acceleration_time = min_acceleration_time + axes_str = ' '.join(('%s' % axis.address for axis in self.real_axes)) + _command(self.controller._cnx, + "#PARACCT {} {}".format(acceleration_time, + axes_str)) + self._acceleration_time = acceleration_time + return acceleration_time + + def _get_acceleration_time(self): + return self._acceleration_time + + def _read_position(self): + rposition = numpy.nan + if self._axes: + axes_str = ' '.join(('%s' % axis.address for axis in self.enabled_axes)) + try: + positions = _command(self.controller._cnx, + "?PARPOS {}".format(axes_str)) + except RuntimeError: + pass # Parametric mode is not in sync + else: + positions = numpy.array([float(pos) for pos in positions.split()]) + rposition = positions.mean() + #update real motors + for axis in self.enabled_axes: + axis._read_dial_and_update() + return rposition + + def _state(self): + axes_str = ' '.join(('%s' % axis.address for axis in self.enabled_axes)) + all_status = [int(s, 16) for s in _command(self.controller._cnx, + "?FSTATUS %s" % (axes_str)).split()] + status = all_status.pop(0) + for axis_status in all_status: + rp_status = status & (axis_status & (1<<9|1<<23)) # READY POWERON + other_status = (status | axis_status) & ~(1<<9|1<<23) + status = rp_status | other_status + return status diff --git a/bliss/controllers/motors/mockup.py b/bliss/controllers/motors/mockup.py index 0c8af08e18a71715794afde9f7550c05f08ad19c..728055a44a8499c495c2772f85b56ae8fb779b6f 100644 --- a/bliss/controllers/motors/mockup.py +++ b/bliss/controllers/motors/mockup.py @@ -5,14 +5,14 @@ # Copyright (c) 2016 Beamline Control Unit, ESRF # Distributed under the GNU LGPLv3. See LICENSE for more info. -import math import time import random import gevent +from bliss.physics.trajectory import LinearTrajectory from bliss.controllers.motor import Controller from bliss.common import log as elog -from bliss.common.axis import Axis, AxisState, MotionEstimation +from bliss.common.axis import Axis, AxisState from bliss.common import event from bliss.common.hook import MotionHook @@ -32,118 +32,6 @@ config : 'backlash' in unit """ -class Trajectory(object): - """ - Trajectory representation for a motion - - v| pa,ta_________pb,tb - | // \\ - |_____//__________\\_______> t - pi,ti pf,tf - <--duration--> - """ - def __init__(self, pi, pf, velocity, acceleration, ti=None): - if ti is None: - ti = time.time() - self.ti = ti - self.pi = pi = float(pi) - self.pf = pf = float(pf) - self.velocity = velocity = float(velocity) - self.acceleration = acceleration = float(acceleration) - self.p = pf - pi - self.dp = abs(self.p) - self.positive = pf > pi - - full_accel_time = velocity / acceleration - full_accel_dp = 0.5 * acceleration * full_accel_time**2 - - full_dp_non_const_vel = 2 * full_accel_dp - self.reaches_top_vel = self.dp > full_dp_non_const_vel - if self.reaches_top_vel: - self.top_vel_dp = self.dp - full_dp_non_const_vel - self.top_vel_time = self.top_vel_dp / velocity - self.accel_dp = full_accel_dp - self.accel_time = full_accel_time - self.duration = self.top_vel_time + 2 * self.accel_time - self.ta = self.ti + self.accel_time - self.tb = self.ta + self.top_vel_time - if self.positive: - self.pa = pi + self.accel_dp - self.pb = self.pa + self.top_vel_dp - else: - self.pa = pi - self.accel_dp - self.pb = self.pa - self.top_vel_dp - else: - self.top_vel_dp = 0 - self.top_vel_time = 0 - self.accel_dp = self.dp / 2 - self.accel_time = math.sqrt(2 * self.accel_dp / acceleration) - self.duration = 2 * self.accel_time - self.velocity = acceleration * self.accel_time - self.ta = self.tb = self.ti + self.accel_time - if self.positive: - pa_pb = pi + self.accel_dp - else: - pa_pb = pi - self.accel_dp - self.pa = self.pb = pa_pb - self.tf = self.ti + self.duration - - def position(self, instant=None): - """Position at a given instant in time""" - if instant is None: - instant = time.time() - if instant < self.ti: - raise ValueError('instant cannot be less than start time') - if instant > self.tf: - return self.pf - dt = instant - self.ti - p = self.pi - f = 1 if self.positive else -1 - if instant < self.ta: - accel_dp = 0.5 * self.acceleration * dt**2 - return p + f * accel_dp - - p += f * self.accel_dp - - # went through the initial acceleration - if instant < self.tb: - t_at_max = dt - self.accel_time - dp_at_max = self.velocity * t_at_max - return p + f * dp_at_max - else: - dp_at_max = self.top_vel_dp - decel_time = instant - self.tb - decel_dp = 0.5 * self.acceleration * decel_time**2 - return p + f * dp_at_max + f * decel_dp - - def instant(self, position): - """Instant when the trajectory passes at the given position""" - d = position - self.pi - dp = abs(d) - if dp > self.dp: - raise ValueError('position outside trajectory') - - dt = self.ti - if dp > self.accel_dp: - dt += self.accel_time - else: - return math.sqrt(2 * dp / self.acceleration) + dt - - top_vel_dp = dp - self.accel_dp - if top_vel_dp > self.top_vel_dp: - # starts deceleration - dt += self.top_vel_time - decel_dp = abs(position- self.pb) - dt += math.sqrt(2 * decel_dp / self.acceleration) - else: - dt += top_vel_dp / self.velocity - return dt - - def __repr__(self): - return '{0}({1.pi}, {1.pf}, {1.velocity}, {1.acceleration}, {1.ti})' \ - .format(type(self).__name__, self) - - class Motion(object): """Describe a single motion""" @@ -156,7 +44,7 @@ class Motion(object): pf = high_limit if pf < low_limit: pf = low_limit - self.trajectory = Trajectory(pi, pf, velocity, acceleration, ti) + self.trajectory = LinearTrajectory(pi, pf, velocity, acceleration, ti) class Mockup(Controller): @@ -561,6 +449,23 @@ class Mockup(Controller): def set_cust_attr_float(self, axis, value): self.__cust_attr_float[axis] = value + + def has_trajectory(self): + return True + + def prepare_trajectory(self, *trajectories): + pass + + def move_to_trajectory(self, *trajectories): + pass + + def start_trajectory(self, *trajectories): + pass + + def stop_trajectory(self, *trajectories): + pass + + class MockupAxis(Axis): def __init__(self, *args, **kwargs): diff --git a/bliss/physics/trajectory.py b/bliss/physics/trajectory.py new file mode 100644 index 0000000000000000000000000000000000000000..012185e63d0f2cd2f0fdeb7018b34692155a7f1e --- /dev/null +++ b/bliss/physics/trajectory.py @@ -0,0 +1,268 @@ +# -*- coding: utf-8 -*- +# +# This file is part of the bliss project +# +# Copyright (c) 2018 Beamline Control Unit, ESRF +# Distributed under the GNU LGPLv3. See LICENSE for more info. + +import time +import math +import numpy + +try: + #this is used only for the b-spline + from scipy import interpolate +except ImportError: + interpolate = None + +class PointTrajectory(object): + """ + class helper to build trajectories. + """ + def __init__(self): + self._time = None + self._positions = dict() + self._velocity = dict() + self._acceleration = dict() + + def build(self, time_array, positions, + spline_nb_points=0): + """ + Build trajectory from given positions and time. + + This will create the velocity and acceleration array + according to the positions given. + if spline_nb_points > 0 the function will use a b-spline + to interpolate. + + Args: + time_array : is a list or numpy array of the time of each points. + positions : is a dictionary where the key is a name and the + value is a numpy array or a list of values. + """ + ys = [numpy.array(y) for y in positions.values()] + xs = [numpy.array(time_array)] + xs.extend(ys) + + if spline_nb_points > 0: + tck, _ = interpolate.splprep(xs, k=3, s=0) + u = numpy.linspace(0, 1, spline_nb_points, endpoint=True) + out = interpolate.splev(u, tck) + else: + out = xs + + self._time = out[0] + + self._positions = dict() + self._velocity = dict() + self._acceleration = dict() + for name, values in zip(positions.keys(), out[1:]): + self._positions[name] = values + velocity = numpy.gradient(values, self._time) + self._acceleration[name] = numpy.gradient(velocity, self._time) + self._velocity[name] = velocity + + def max_velocity(self): + """ + Return the maximum velocity. + """ + max_vel = dict() + for name, velocities in self._velocity.iteritems(): + max_vel[name] = numpy.absolute(velocities).max() + return max_vel + + def max_acceleration(self): + """ + Return the maximum acceleration. + """ + max_acc = dict() + for name, accelerations in self._acceleration.iteritems(): + max_acc[name] = numpy.absolute(accelerations).max() + return max_acc + + def limits(self): + """ + Return the min and max position for this movement. + Can be easily compared with the motor limits + """ + limits = dict() + for name, positions in self._positions.iteritems(): + limits[name] = (positions.min(), positions.max()) + return limits + + def pvt(self, acceleration_start_end=None): + """ + Get PVT vectors into named dictionary. + + Each vectors is a numpy struct with 3 columns ('time','position','velocity') + + Keyword arguments:: + acceleration_start_end -- is a dictionary with the maximum acceleration + if not None will add two points + to the given trajectories to starts and ends with no velocity. + if the maximum acceleration is given it will be used it for this + new points. otherwise maximum acceleration on the trajectory will be + used. + """ + if self._time is None or not self._time.size: + raise RuntimeError("No trajectory built, call build method first") + + nb_point = len(self._time) if acceleration_start_end is None \ + else len(self._time) + 2 + + pvt_arrays = dict() + + for name, positions, velocities in zip(self._positions.keys(), + self._positions.values(), + self._velocity.values()): + dtype = [('time', 'f8'), ('position', positions.dtype), + ('velocity', velocities.dtype)] + pvt_arrays[name] = numpy.zeros(nb_point, dtype) + + if acceleration_start_end is not None: + max_acc = self.max_acceleration() + max_acc.update(acceleration_start_end) + + max_acc_time = 0. + for name, velocities in self._velocity.iteritems(): + velocity = max(abs(velocities[0]), abs(velocities[-1])) + acc = max_acc[name] + acc_time = velocity/acc + if acc_time > max_acc_time: + max_acc_time = acc_time + + for name, positions, velocities in zip(self._positions.keys(), + self._positions.values(), + self._velocity.values()): + pvt_array = pvt_arrays[name] + pvt_array['time'][1:-1] = self._time + max_acc_time + pvt_array['time'][-1] = pvt_array['time'][-2] + max_acc_time + + pvt_array['velocity'][1:-1] = velocities + + pvt_array['position'][1:-1] = positions + first_point = positions[0] - (velocities[0] * max_acc_time / 2.) + last_point = positions[-1] + (velocities[-1] * max_acc_time / 2.) + pvt_array['position'][0] = first_point + pvt_array['position'][-1] = last_point + else: + for name, positions, velocities in zip(self._positions.keys(), + self._positions.values(), + self._velocity.values()): + pvt_array = pvt_arrays[name] + pvt_array['time'] = self._time + pvt_array['position'] = positions + pvt_array['velocity'] = velocities + + return pvt_arrays + +class LinearTrajectory(object): + """ + Trajectory representation for a linear motion + + v| pa,ta_________pb,tb + | // \\ + |_____//__________\\_______> t + pi,ti pf,tf + <--duration--> + """ + def __init__(self, pi, pf, velocity, acceleration, ti=None): + if ti is None: + ti = time.time() + self.ti = ti + self.pi = pi = float(pi) + self.pf = pf = float(pf) + self.velocity = velocity = float(velocity) + self.acceleration = acceleration = float(acceleration) + self.p = pf - pi + self.dp = abs(self.p) + self.positive = pf > pi + + full_accel_time = velocity / acceleration + full_accel_dp = 0.5 * acceleration * full_accel_time**2 + + full_dp_non_const_vel = 2 * full_accel_dp + self.reaches_top_vel = self.dp > full_dp_non_const_vel + if self.reaches_top_vel: + self.top_vel_dp = self.dp - full_dp_non_const_vel + self.top_vel_time = self.top_vel_dp / velocity + self.accel_dp = full_accel_dp + self.accel_time = full_accel_time + self.duration = self.top_vel_time + 2 * self.accel_time + self.ta = self.ti + self.accel_time + self.tb = self.ta + self.top_vel_time + if self.positive: + self.pa = pi + self.accel_dp + self.pb = self.pa + self.top_vel_dp + else: + self.pa = pi - self.accel_dp + self.pb = self.pa - self.top_vel_dp + else: + self.top_vel_dp = 0 + self.top_vel_time = 0 + self.accel_dp = self.dp / 2 + self.accel_time = math.sqrt(2 * self.accel_dp / acceleration) + self.duration = 2 * self.accel_time + self.velocity = acceleration * self.accel_time + self.ta = self.tb = self.ti + self.accel_time + if self.positive: + pa_pb = pi + self.accel_dp + else: + pa_pb = pi - self.accel_dp + self.pa = self.pb = pa_pb + self.tf = self.ti + self.duration + + def position(self, instant=None): + """Position at a given instant in time""" + if instant is None: + instant = time.time() + if instant < self.ti: + raise ValueError('instant cannot be less than start time') + if instant > self.tf: + return self.pf + dt = instant - self.ti + p = self.pi + f = 1 if self.positive else -1 + if instant < self.ta: + accel_dp = 0.5 * self.acceleration * dt**2 + return p + f * accel_dp + + p += f * self.accel_dp + + # went through the initial acceleration + if instant < self.tb: + t_at_max = dt - self.accel_time + dp_at_max = self.velocity * t_at_max + return p + f * dp_at_max + else: + dp_at_max = self.top_vel_dp + decel_time = instant - self.tb + decel_dp = 0.5 * self.acceleration * decel_time**2 + return p + f * dp_at_max + f * decel_dp + + def instant(self, position): + """Instant when the trajectory passes at the given position""" + d = position - self.pi + dp = abs(d) + if dp > self.dp: + raise ValueError('position outside trajectory') + + dt = self.ti + if dp > self.accel_dp: + dt += self.accel_time + else: + return math.sqrt(2 * dp / self.acceleration) + dt + + top_vel_dp = dp - self.accel_dp + if top_vel_dp > self.top_vel_dp: + # starts deceleration + dt += self.top_vel_time + decel_dp = abs(position- self.pb) + dt += math.sqrt(2 * decel_dp / self.acceleration) + else: + dt += top_vel_dp / self.velocity + return dt + + def __repr__(self): + return '{0}({1.pi}, {1.pf}, {1.velocity}, {1.acceleration}, {1.ti})' \ + .format(type(self).__name__, self) diff --git a/bliss/scanning/acquisition/motor.py b/bliss/scanning/acquisition/motor.py index 3e276f129656a44feae8d2426cb9e01c7bbb45a9..36d87f3c887e2d803d90e53716de72519ab00253 100644 --- a/bliss/scanning/acquisition/motor.py +++ b/bliss/scanning/acquisition/motor.py @@ -379,3 +379,33 @@ class VariableStepTriggerMaster(AcquisitionMaster): [axis.position() for axis in self._axes]) self.wait_slaves() + +class TrajectoryMaster(AcquisitionMaster): + def __init__(self, axis, start, end, nb_points, time_per_point, + trigger_type=AcquisitionMaster.HARDWARE, + type="axis", **keys): + AcquisitionMaster.__init__(self, axis, axis.name, type, + trigger_type=trigger_type, **keys) + self.movable = axis + self.trajectory = axis.scan_on_trajectory(start, end, nb_points, time_per_point) + + def prepare(self): + self.trajectory.prepare() + self.trajectory.move_to_start() + + def start(self): + if self.trigger_type == AcquisitionMaster.SOFTWARE: + return + self.trigger() + + def trigger(self): + if self.trigger_type == AcquisitionMaster.SOFTWARE: + self.trigger_slaves() + + self.trajectory.move_to_end() + + def wait_ready(self): + self.trajectory.wait_move() + + def stop(self): + self.trajectory.stop() diff --git a/bliss/scanning/scan.py b/bliss/scanning/scan.py index 536164858c93da417e7423f4d5de6e13f135b0d8..a7f0aac6fe4145f805efe798f779440a97074ff4 100644 --- a/bliss/scanning/scan.py +++ b/bliss/scanning/scan.py @@ -359,7 +359,8 @@ class Scan(object): except BaseException as exc: self._state = self.STOP_STATE with periodic_exec(0.1 if call_on_stop else 0, set_watch_event): - i.stop() + if i is not None: + i.stop() raise else: self._state = self.STOP_STATE diff --git a/bliss/shell/__init__.py b/bliss/shell/__init__.py index 4db17f2771c9739d7496ed4c4ffffd1b7a0e191b..7cb02c63a9c59a5c73a66a02c0268a6522ef5d3b 100644 --- a/bliss/shell/__init__.py +++ b/bliss/shell/__init__.py @@ -120,8 +120,10 @@ class ScanListener: scan_info = dict(scan_info) self.term = term = Terminal(scan_info.get('stream')) scan_info = dict(scan_info) - - motors = scan_info['motors'] + try: + motors = scan_info['motors'] + except KeyError: # @todo remove this + return # silently ignoring for continuous scan counters = scan_info['counters'] nb_points = scan_info['npoints'] if not scan_info['save']: @@ -219,7 +221,12 @@ class ScanListener: print_(line) def __on_scan_end(self, scan_info): - if scan_info['type'] == 'ct': + try: + scan_type = scan_info['type'] + except KeyError: # @todo remove this + return # silently ignoring for continuous scan + + if scan_type == 'ct': return for motor in self.real_motors: diff --git a/tests/motors/test_axis.py b/tests/motors/test_axis.py index 1da903e0946e939188da599509dab2341869e985..73580a76c793f67ad57a6d21b137fb3477794ab4 100644 --- a/tests/motors/test_axis.py +++ b/tests/motors/test_axis.py @@ -201,7 +201,7 @@ def test_limits(robz): def test_limits2(robz, roby): iset_pos = robz._set_position() assert robz.limits() == (-1000,1E9) - assert roby.limits() == (None,None) + assert roby.limits() == (float('-inf'),float('+inf')) with pytest.raises(ValueError): robz.move(-1001) assert robz._set_position() == iset_pos @@ -472,7 +472,7 @@ def test_apply_config(roby): roby.apply_config() assert roby.velocity() == 2500 assert roby.acceleration() == 1000 - assert roby.limits() == (None, None) + assert roby.limits() == (float('-inf'), float('+inf')) def test_jog(robz): robz.velocity(10) diff --git a/tests/motors/test_mockup.py b/tests/motors/test_mockup.py index a51f26de4031013b4bb9df08ad8cb400f6218e3c..acd04b6aa851c0c1a62c73e19f2d75bc8309a4fe 100644 --- a/tests/motors/test_mockup.py +++ b/tests/motors/test_mockup.py @@ -5,7 +5,7 @@ from collections import OrderedDict import pytest -from bliss.controllers.motors.mockup import Trajectory +from bliss.physics.trajectory import LinearTrajectory parameters = [ @@ -64,7 +64,7 @@ parameters = [ for param in parameters], ids=[param['desc'] for param in parameters]) def test_trajectory(motion, expected_trajectory): - traj = Trajectory(**motion) + traj = LinearTrajectory(**motion) for param, value in expected_trajectory.items(): assert value == pytest.approx(getattr(traj, param), param) @@ -75,7 +75,7 @@ def test_trajectory(motion, expected_trajectory): for param in parameters], ids=[param['desc'] for param in parameters]) def test_trajectory_instant(motion, positions, expected_instants): - traj = Trajectory(**motion) + traj = LinearTrajectory(**motion) for position, expected_instant in zip(positions, expected_instants): assert traj.instant(position) == pytest.approx(expected_instant) @@ -86,7 +86,7 @@ def test_trajectory_instant(motion, positions, expected_instants): for param in parameters], ids=[param['desc'] for param in parameters]) def test_trajectory_positions(motion, instants, expected_positions): - traj = Trajectory(**motion) + traj = LinearTrajectory(**motion) for instant, expected_position in zip(instants, expected_positions): assert traj.position(instant) == pytest.approx(expected_position) diff --git a/tests/motors/test_trajectories.py b/tests/motors/test_trajectories.py new file mode 100644 index 0000000000000000000000000000000000000000..8eed67b0a2f78d53edeacd9158b90497f1cf512d --- /dev/null +++ b/tests/motors/test_trajectories.py @@ -0,0 +1,72 @@ +# -*- coding: utf-8 -*- +# +# This file is part of the bliss project +# +# Copyright (c) 2016 Beamline Control Unit, ESRF +# Distributed under the GNU LGPLv3. See LICENSE for more info. + +import pytest +import time +import gevent +import gevent.event +from bliss.common import event +import numpy +import mock + +def test_traj_from_calc(s1hg, s1b, s1f, s1u, s1d): + tg = s1hg.scan_on_trajectory(0, 5, 100, 0.01) + trajectories = tg.trajectories + + assert set([a.name for a in tg.axes]) == set(['s1u', 's1d', 's1f', 's1b']) + + for traj in trajectories: + if traj.axis.name in ('s1u', 's1d'): + assert not numpy.any(traj.pvt['position']) + elif traj.axis.name in ('s1f', 's1b'): + assert pytest.approx(traj.pvt[:-1]['position'], 2.5) + assert len(traj.pvt) == 100+2 #include start, final extra points for traj. + + assert len(tg.disabled_axes) == 0 + + assert tg.calc_axis == s1hg + + s1hg.dial(-1) + assert s1f.offset == -.5 + assert s1b.offset == -.5 + + tg.prepare() + assert tg._TrajectoryGroup__trajectories_dialunit + for i, traj in enumerate(tg._TrajectoryGroup__trajectories_dialunit): + if traj.axis.name in ('s1u', 's1d'): + assert not numpy.any(traj.pvt['position']) + elif traj.axis.name in ('s1f', 's1b'): + user_pos_traj = trajectories[i].pvt['position']*traj.axis.steps_per_unit + assert numpy.allclose(user_pos_traj-traj.pvt['position'], [-.5*traj.axis.steps_per_unit]*102) + + tg.move_to_start() + + tg.disable_axis(s1u) + tg.disable_axis(s1d) + assert len(tg.disabled_axes) == 2 + assert set(tg.disabled_axes) == set([s1u, s1d]) + def check_trajectories(*trajectories): + assert len(trajectories) == 2 + with mock.patch.object(s1f.controller, "prepare_trajectory", check_trajectories): + tg.prepare() + + tg.enable_axis(s1d) + assert len(tg.disabled_axes) == 1 + assert set(tg.disabled_axes) == set([s1u]) + def check_trajectories(*trajectories): + assert len(trajectories) == 3 + with mock.patch.object(s1f.controller, "prepare_trajectory", check_trajectories): + tg.prepare() + + tg.move_to_end() + +def test_traj_from_calc_from_calc(calc_mot2): + tg = calc_mot2.scan_on_trajectory(0, 1, 100, 0.1) + trajectories = tg.trajectories + + assert set([t.axis.name for t in trajectories]) == set(['roby']) +