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)
