def __init__(self, position: Optional[Vector] = None, dcm: Optional[Matrix] = None, velocity: Optional[Vector] = None, angular_velocity: Optional[Vector] = None, acceleration: Optional[Vector] = None, angular_acceleration: Optional[Vector] = None, time: Optional[Num] = np_.NaN, linear: Optional[_linear.Linear] = None, angular: Optional[_angular.Angular] = None, **kwargs): super().__init__(**kwargs) # no linear object given, then build it from the arguments and their # defaults if linear is None: linear = _linear.Linear(position, velocity, acceleration) # no angular object given, then build it from the arguments and their # defaults if angular is None: angular = _angular.Angular(dcm, angular_velocity, angular_acceleration) # assign processed properties self.linear = linear self.angular = angular self.time = time
def random_2r3t(num: int = None): if num is None or num == 1: return Pose(linear=_linear.Linear.random(dim=2), angular=_angular.Angular(sequence='xy', euler=np_.pi * (np_.random.random(2) - 0.5))) else: return (PoseGenerator.random_2t() for _ in range(num))
def __init__(self, translation: Optional[Vector] = None, dcm: Optional[Matrix] = None, **kwargs): super().__init__(**kwargs) # init internal variables self.linear = _linear.Linear() self.angular = _angular.Angular() # and set values self.translation = translation if translation is not None else [ 0, 0, 0 ] self.dcm = dcm if dcm is not None else np_.eye(3)
def __init__(self, radius: Num, geometry: Optional[_geometry.Primitive] = None, inertia: Optional[_inertia.Inertia] = None, dcm: Optional[Matrix] = None, angular: Optional[_angular.Angular] = None, **kwargs): super().__init__(**kwargs) self.radius = radius self.geometry = geometry or None self.inertia = inertia or _inertia.Inertia() if angular is None: angular = _angular.Angular( dcm=dcm if dcm is not None else np_.eye(3)) self.angular = angular
def goal_function(x: Vector, robot_: _robot.Robot, xpose_: _pose.Pose, *args, **kwargs): # position estimate xpose_.linear.position = x[0:3] # euler angle estimate to rotation matrix xpose_.angular = _angular.Angular(sequence='xyz', euler=x[3:6]) # solve the vector loop and obtain solution estim_lengths, directions, _ = self._vector_loop(robot_, xpose_) # number of linear degrees of freedom num_linear, _ = robot.num_dimensionality # strip additional spatial dimensions directions = directions[:, 0:num_linear] # store last direction so we have it later last_direction.insert(0, directions) # return error as (l^2 - l(x)^2) return lengths**2 - estim_lengths**2
def __init__(self, position: Optional[Vector] = None, dcm: Optional[Matrix] = None, linear: Optional[_linear.Linear] = None, angular: Optional[_angular.Angular] = None, **kwargs): """ Parameters ---------- position : Vector (3,) vector of the (relative) position of the anchor in a reference coordinate system dcm : Matrix (3,3) matrix representing the rotation matrix `dcm` of the platform anchor. Use only where applicable. linear : Linear Linear transformation object that is to be used instead of `position` if given. angular : Angular Angular transformation object that is to be used instead of `rotation` if given. """ super().__init__(**kwargs) # initialize and set linear property if not given by the user if linear is None: linear = _linear.Linear( position if position is not None else [0.0, 0.0, 0.0]) # set linear transformation to inferred value self.linear = linear # initialize and set angularlinear property if not given by the user if angular is None: angular = _angular.Angular( dcm=dcm if dcm is not None else np_.eye(3)) # set linear transformation to inferred value self.angular = angular
def _forward_goal_function(self, x: Vector, robot: _robot.Robot, pose: _pose.Pose, joints: Vector, *args, **kwargs): # position estimate pose.linear.position = x[0:3] # euler angle estimate to rotation matrix pose.angular = _angular.Angular(sequence='xyz', euler=x[3:6]) # solve the vector loop and obtain solution estim_lengths, directions, *_ = self._vector_loop(robot, pose) estim_lengths = _np.sum(estim_lengths, axis=1) # number of linear degrees of freedom num_linear, _ = robot.num_dimensionality # strip additional spatial dimensions directions = directions[:, 0:num_linear] # store last direction so we have it later self._forward_last_direction = directions # return error as (l^2 - l(x)^2) return joints**2 - estim_lengths**2
def _vector_loop(self, robot: _robot.Robot, pose: _pose.Pose, *args, **kwargs): # type hinting fanchor: _frame.FrameAnchor panchor: _platform.PlatformAnchor pulley: _pulley.Pulley cable: _cable.Cable kc: _kinematicchain.KinematicChain # init results swivel = [] wrap = [] lengths = [] directions = [] cable_leave_points = [] # number of linear dimensionality num_lin, _ = robot.num_dimensionality # loop over each kinematic chain for idxkc, kc in enumerate(robot.kinematic_chains): fanchor = robot.frame.anchors[kc.frame_anchor] platform = robot.platforms[kc.platform] panchor = platform.anchors[kc.platform_anchor] # cable = robot.cables[chain.cable] pulley = fanchor.pulley frame_dcm = fanchor.angular.dcm pulley_dcm = pulley.dcm # extract platform position and orientation ppos, prot = pose.position # frame anchor to platform (negative of conventional vector loop) frame_to_platform = ppos \ + prot.dot(panchor.linear.position) \ - fanchor.linear.position # pulley to platform pulley_to_platform = pulley_dcm.T.dot( frame_dcm.T.dot(frame_to_platform)) # calculate angle of swivel swivel_ = _np.arctan2(pulley_to_platform[1], pulley_to_platform[0]) swivel.append(swivel_) # rotation matrix from pulley to cable coordinate system cable_dcm = _angular.Angular.rotation_z(swivel_).dcm # position of the platform with respect to the roller center roller_center_to_platform = cable_dcm.T.dot( pulley_to_platform) - _np.asarray([pulley.radius, 0, 0]) # calculate length of cable in workspace length_workspace = _np.abs(_np.sqrt(_np.linalg.norm( roller_center_to_platform) ** 2 - pulley.radius ** 2)) # build matrix of cable workspace length and pulley radius leave_dcm = _np.asarray(( (pulley.radius, length_workspace), (-length_workspace, pulley.radius), )) # position of the cable leave point in coordinates of the roller # center leave_point = _np.linalg.solve(leave_dcm, roller_center_to_platform[[0, 2]]) # "unwrapped angle" unwrapped = _np.arctan2(leave_point[1], leave_point[0]) cable_leave_points.append(leave_point[0:num_lin]) # append the angle of wrap wrap_ = _np.pi - unwrapped wrap.append(wrap_) # length of cable on the roller length_roller = pulley.radius * wrap_ # store length as result lengths.append([length_workspace, length_roller]) # calculate vector of cable leave point relative to the cable # coordinate system direction = (frame_dcm.dot(pulley_dcm.dot(cable_dcm.dot( [pulley.radius, 0, 0] + _angular.Angular().rotation_y( wrap_).dcm.dot([-pulley.radius, 0, 0])))) + ( fanchor.linear.position - ( ppos + prot.dot( panchor.linear.position)))) # direction = direction[0:num_lin] / length_workspace directions.append(direction[0:num_lin] / length_workspace) # turn everything into numpy arrays lengths = _np.asarray(lengths) directions = _np.asarray(directions) swivel = _np.asarray(swivel) wrap = _np.asarray(wrap) leave_points = _np.asarray(cable_leave_points) return lengths, directions, leave_points, swivel, wrap
def orientation(start: Union[Num, Vector], end: Union[Num, Vector], sequence: AnyStr, position: Optional[Vector] = None, steps: Union[None, Num, Vector] = None): """ Generator for purely orientational changing poses Parameters ---------- start : Num | Vector Orientation given in Euler angles at which the orientation-only pose generator should start. end : Num | Vector Orientation given in Euler angles at which the orientation-only pose generator should end. Must be the same size as `start`. sequence : AnyStr Sequence of Euler orientations used to reconstruct the orientation matrix. Can be any valid combination of intrinsic `(x, y, z)` or extrinsic `(X, Y, Z)`. Number of rotations must match the number of start Euler angles. position : Num | Vector Fix position vector at which the orientational poses should be applied. Any non `(3,)` vector or scalar will be padded up to dimension `(3,)`. steps : Num | Vector | N-tuple Number of discretization steps from `start` to `end`. If given as number, will be applied to all dimensions of start, otherwise must match the size of `start`. Returns ------- """ # by default, we will make 10 steps steps = steps if steps is not None else 5 # convert all into numpy arrays start = np_.asarray(start) end = np_.asarray(end) steps = np_.asarray(steps, dtype=np_.int) if start.ndim == 0: start = np_.asarray([start]) if end.ndim == 0: end = np_.asarray([end]) if steps.ndim == 0: steps = np_.asarray([steps], dtype=np_.int) # count the number of dimensions num_euler = start.size # ensure sequence length matches the number of start euler angles _validator.data.length(sequence, num_euler, 'sequence') # ensure `end` has the right size _validator.data.length(end, num_euler, 'end') # if `step` is given with only one dimension, pad it to match the number # of dimensions if steps.size < num_euler: steps = np_.repeat(steps, num_euler - (steps.size - 1)) # ensure `step` now has the right size _validator.data.length(steps, num_euler, 'step') # no rotation matrix given, then take unity if position is None: position = np_.asarray([0.0, 0.0, 0.0]) # right-pad position with zeros so that `Pose` won't throw an error position = np_.pad(position, (0, 3 - position.size)) # repeat step to match the amount of rotations to do steps = np_.repeat(steps, start.size - (steps.size - 1))[0:num_euler] # calculate difference between `start` and `end` position diff = end - start # and delta per step delta = diff / steps # remove numeric artifacts and set to `0` where there must be no steps delta[np_.isclose(steps, 0)] = 0 # how many iterations to perform per axis iterations = steps * np_.logical_not(np_.isclose(diff, 0)) # return an iterator object return (Pose(position, angular=_angular.Angular(sequence=sequence, euler=start + delta * step)) for step in itertools.product(*(range(k + 1) for k in iterations)))
def full(position: Tuple[Union[Num, Vector], Union[Num, Vector]], angle: Tuple[Union[Num, Vector], Union[Num, Vector]], sequence: str, steps: Union[Num, Tuple[Union[Num, Vector], Union[Num, Vector]]] = 10): # Default arguments for the steps steps = steps if steps is not None else 5 # ensure both position values are numpy arrays position = [ np_.asarray(x if isinstance(x, Iterable) else [x]) for x in position ] # ensure both angle values are numpy arrays angle = [ np_.asarray(x if isinstance(x, Iterable) else [x]) for x in angle ] # now make sure both start and end position have the same size _validator.data.length(position[1], position[0].size, 'position[1]') # and make sure both start and end angles have the size given through # the # sequence [ _validator.data.length(a, len(sequence), f'angle[{idx}]') for idx, a in enumerate(angle) ] # count values nums = (position[0].size, angle[0].size) # if steps is not an iterable object, we will make it one if not isinstance(steps, Iterable): steps = (steps, steps) # at this point, steps is either a 2-tuple of (steps[pos], steps[angle]) # or it is a 2-tuple of ([steps[pos_0], ..., steps[pos_n]], [steps[ # angle_0], ..., steps[angle_n]]) so let's check for that steps = [ np_.asarray( step if isinstance(step, Iterable) else np_.repeat(step, num)) for num, step in zip(nums, steps) ] # check steps has the right dimensions now, should be count ((Np, ), # (Na, )) [ _validator.data.length(step, nums[idx], f'steps[{idx}]') for idx, step in enumerate(steps) ] # calculate the delta per step for each degree of freedom deltas = [(v[1] - v[0]) / step for v, step in zip((position, angle), steps)] # set deltas to zero where there is no step needed for idx in range(2): deltas[idx][np_.logical_or(np_.allclose(steps[idx], 0), np_.isnan(deltas[idx]))] = 0 # at this point, we must ensure that the values for `position` are all # `(3,)` arrays position = [np_.pad(pos, (0, 3 - pos.size)) for pos in position] # from this follows, that we must also ensure that `steps[0]` now # contains 3 elements since the position now has three elements steps[0] = np_.pad(steps[0], (0, 3 - steps[0].size)) deltas[0] = np_.pad(deltas[0], (0, 3 - deltas[0].size)) # Finally, return the iterator object return (Pose(position[0] + deltas[0] * step[0:3], angular=_angular.Angular(sequence=sequence, euler=angle[0] + deltas[1] * step[3:])) for step in itertools.product( *(range(k + 1) for k in itertools.chain(*steps))))
def random_3t(num: int = None): if num is None or num == 1: return Pose(linear=_linear.Linear.random(dim=3), angular=_angular.Angular()) else: return (PoseGenerator.random_3t() for _ in range(num))
def _forward(self, robot: _robot.Robot, lengths: Vector, **kwargs) -> _algorithm.Result: # for now, this is the inner-loop code for the first platform index_platform = 0 # consistent arguments lengths = _np.asarray(lengths) # quicker and shorter access to platform object platform = robot.platforms[index_platform] # kinematic chains of the platform kcs = robot.kinematic_chains.with_platform(index_platform) # get frame anchors and platform anchors frame_anchors = _np.asarray([ robot.frame.anchors[anchor_index].linear.position for anchor_index in kcs.frame_anchor ]) platform_anchors = _np.asarray([ platform.anchors[anchor_index].linear.position for anchor_index in kcs.platform_anchor ]) # initial directions needed for later returned result last_direction = [] # goal function for the estimator def goal_function(x: Vector, robot_: _robot.Robot, xpose_: _pose.Pose, *args, **kwargs): # position estimate xpose_.linear.position = x[0:3] # euler angle estimate to rotation matrix xpose_.angular = _angular.Angular(sequence='xyz', euler=x[3:6]) # solve the vector loop and obtain solution estim_lengths, directions, _ = self._vector_loop(robot_, xpose_) # number of linear degrees of freedom num_linear, _ = robot.num_dimensionality # strip additional spatial dimensions directions = directions[:, 0:num_linear] # store last direction so we have it later last_direction.insert(0, directions) # return error as (l^2 - l(x)^2) return lengths**2 - estim_lengths**2 # initial pose estimate given by user? initial_estimate = kwargs.pop('x0', None) # no initial pose estimate given, so calculate one based on Schmidt.2016 if initial_estimate is None: initial_estimate = self._pose_estimate(robot, lengths) # scaling of all parameters to increase sensitivity of the optimizer x_scale = kwargs.pop('x_scale', None) if x_scale is None: x_scale = _np.asarray([0.01, 0.01, 0.01, 100.0, 100.0, 100.0]) # bounds of estimation bounds = ( [ -_np.inf, -_np.inf, -_np.inf, # [x, y, z] -0.5 * _np.pi, -0.5 * _np.pi, -0.5 * _np.pi ], # [r, p, y] [ _np.inf, _np.inf, _np.inf, # [x, y, z] 0.5 * _np.pi, 0.5 * _np.pi, 0.5 * _np.pi ]) # [r, p, y] # minimization method to use method: str = kwargs.pop('method', 'lm') # tolerances for solver ftol = _np.sqrt(_np.finfo(float).eps) / 1e5 xtol = _np.sqrt(_np.finfo(float).eps) / 1e5 gtol = 0.0 # # default keyword arguments to `least_squares` # defaults = { # 'method': method, # 'ftol': ftol, # 'xtol': xtol, # 'gtol': gtol, # 'x_scale': x_scale, # 'jac': '3-point', # 'args': ( # frame_anchors, # platform_anchors, # ), # 'kwargs': { # 'ai': frame_anchors, # 'bi': platform_anchors # }, # } # # only add `bounds` if the solver is not `lm` (levenberg-marquardt) # if method != 'lm': # defaults['bounds'] = bounds # # update with user-supplied kwargs # defaults.update(kwargs) # a pose estimate x_pose = _pose.ZeroPose # estimate pose result: optimize.OptimizeResult result = optimize.least_squares(goal_function, initial_estimate, args=(robot, x_pose), **kwargs) # check for convergence try: if result.success is not True: raise ArithmeticError( f'Optimizer did not exit successfully. Last message ' f'was {result.message}') except ArithmeticError as ArithmeticE: raise ValueError( 'Unable to solve the standard forward kinematics for the ' 'given cable lengths. Please check if your inputs are ' 'correct and then run again. You may also pass additional ' 'arguments to the underlying optimization method.') from \ ArithmeticE else: # get last value final = result.x # make sure rotation is limited to [0, 2*pi) final[3:6] = _np.fmod(final[3:6], 2 * _np.pi) # populate values of estimated pose x_pose.linear.position = final[0:3] x_pose.angular = _angular.Angular(sequence='xyz', euler=final[3:6]) # and return estimated result return _algorithm.Result(self, robot, x_pose, lengths=lengths, directions=last_direction[0], leave_points=frame_anchors)
def test_works_as_expected(self, position: Vector, angle: Vector, sequence: AnyStr, steps: Union[Num, Vector]): # get a pose generator actual_poses = pose.PoseGenerator.full(position, angle, sequence, steps) # Default arguments for the steps steps = 5 if steps is None else steps # ensure both position values are numpy arrays position = [ np.asarray(x if isinstance(x, Iterable) else [x]) for x in position ] # ensure both angle values are numpy arrays angle = [ np.asarray(x if isinstance(x, Iterable) else [x]) for x in angle ] # count values nums = (position[0].size, angle[0].size) # if steps is not an iterable object, we will make it one if not isinstance(steps, Iterable): steps = (steps, steps) # at this point, steps is either a 2-tuple of (steps[pos], steps[angle]) # or it is a 2-tuple of ([steps[pos_0], ..., steps[pos_n]], [steps[ # angle_0], ..., steps[angle_n]]) so let's check for that steps = [ np.asarray( step if isinstance(step, Iterable) else np.repeat(step, num)) for num, step in zip(nums, steps) ] # calculate the delta per step for each degree of freedom deltas = [(v[1] - v[0]) / step for v, step in zip((position, angle), steps)] # set deltas to zero where there is no step needed for idx in range(2): deltas[idx][np.logical_or(np.allclose(steps[idx], 0), np.isnan(deltas[idx]))] = 0 # at this point, we must ensure that the values for `position` are all # `(3,)` arrays position = [np.pad(pos, (0, 3 - pos.size)) for pos in position] # from this follows, that we must also ensure that `steps[0]` now # matches # the size of the positions steps[0] = np.pad(steps[0], (0, 3 - steps[0].size)) deltas[0] = np.pad(deltas[0], (0, 3 - deltas[0].size)) # Finally, return the iterator object expected_poses = (pose.Pose(position[0] + deltas[0] * step[0:3], angular=angular.Angular( sequence=sequence, euler=angle[0] + deltas[1] * step[3:])) for step in itertools.product( *(range(k + 1) for k in itertools.chain(*steps)))) for actual, expected in zip(actual_poses, expected_poses): assert actual == expected
def forward(self, robot: _robot.Robot, joints: Matrix, **kwargs) -> Result: if robot.num_platforms > 1: raise NotImplementedError( 'Kinematics are currently not implemented for robots with ' 'more than one platform.') # for now, this is the inner-loop code for the first platform index_platform = 0 # consistent arguments joints = _np.asarray(joints) # quicker and shorter access to platform object platform = robot.platforms[index_platform] # kinematic chains of the platform kcs = robot.kinematic_chains.with_platform(index_platform) # get frame anchors and platform anchors frame_anchors = _np.asarray([ robot.frame.anchors[anchor_index].linear.position for anchor_index in kcs.frame_anchor ]) # initial directions needed for later returned result last_direction = [] # initial pose estimate given by user? initial_estimate = kwargs.pop('x0', None) # no initial pose estimate given, so calculate one based on Schmidt.2016 if initial_estimate is None: initial_estimate = self._pose_estimate(robot, joints) # a pose estimate x_pose = _pose.ZeroPose # extract forward kinematics goal function from the kwargs, or default # to our own implementation goal_function = kwargs.pop('goal_function', self._forward_goal_function) # estimate pose result: optimize.OptimizeResult result = optimize.least_squares(goal_function, initial_estimate, args=(robot, x_pose, joints), **kwargs) # check for convergence try: if result.success is not True: raise ArithmeticError( f'Optimizer did not exit successfully. Last message ' f'was {result.message}') except ArithmeticError as ArithmeticE: raise ValueError( 'Unable to solve the standard forward kinematics for the ' 'given cable lengths. Please check if your inputs are ' 'correct and then run again. You may also pass additional ' 'arguments to the underlying optimization method.') from \ ArithmeticE else: # get last value final = result.x # make sure rotation is limited to [0, 2*pi) final[3:6] = _np.fmod(final[3:6], 2 * _np.pi) # populate values of estimated pose x_pose.linear.position = final[0:3] x_pose.angular = _angular.Angular(sequence='xyz', euler=final[3:6]) # get and reset last forward direction last_direction = self._forward_last_direction self._forward_last_direction = None # and return estimated result return Result(self, robot, x_pose, lengths=joints, directions=last_direction, leave_points=frame_anchors)