import pickle
from copy import deepcopy
from time import time
from typing import Union
from warnings import warn
import numpy
import sympy
from .DualQuaternion import DualQuaternion
from .Linkage import LineSegment
from .MotionFactorization import MotionFactorization
from .NormalizedLine import NormalizedLine
from .PointHomogeneous import PointHomogeneous
from .RationalCurve import RationalCurve
from .TransfMatrix import TransfMatrix
[docs]
class RationalMechanism(RationalCurve):
"""
Class representing rational mechanisms in dual quaternion space.
Attributes
----------
factorizations : list of MotionFactorization
List of MotionFactorization objects.
num_joints : int
Number of joints in the mechanism.
is_linkage : bool
True if the mechanism is a linkage, False if it is one branch of a linkage.
tool_frame : DualQuaternion
End effector of the mechanism.
metric : AffineMetric
Object representing the metric of the mechanism.
segments : list of LineSegment
List of LineSegment objects representing the physical realization of the linkage.
Examples
--------
.. code-block:: python
# Create a rational mechanism from given example
from rational_linkages import RationalMechanism, Plotter, TransfMatrix
from rational_linkages.models import bennett_ark24
# load the model of the Bennett's linkage
m = bennett_ark24()
# create an interactive plotter object, with 500 discrete steps
# for the input rational curves, and arrows scaled to 0.05 length
p = Plotter(mechanism=m, steps=500, arrows_length=0.05)
##### additional plotting options #####
# create a pose of the identity
base = TransfMatrix()
p.plot(base)
# create another pose
p0 = TransfMatrix.from_rpy_xyz([-90, 0, 0], [0.15, 0, 0], unit='deg')
p.plot(p0)
# show the plot
p.show()
.. clear-namespace::
"""
def __init__(self, factorizations: list[MotionFactorization],
tool: Union[DualQuaternion, str] = None):
"""
Initialize a RationalMechanism object.
Parameters
----------
factorizations : list of MotionFactorization
List of MotionFactorization objects.
tool : DualQuaternion or str, optional
Tool frame of the mechanism. Default is None.
"""
super().__init__(factorizations[0].set_of_polynomials)
self.factorizations = factorizations
self.num_joints = sum([f.number_of_factors for f in factorizations])
self.tool_frame = self._determine_tool(tool)
self.is_linkage = len(self.factorizations) == 2
self._segments = None
self._metric = None
self._linear_motions_cycle = None
@property
def segments(self):
"""
Return the line segments of the linkage.
Line segments are the physical realization of the linkage.
Returns
-------
list of LineSegment
List of LineSegment objects.
Raises
------
ValueError
If segments are accessed for non-linkages.
"""
if self._segments is None and self.is_linkage:
self._segments = self._get_line_segments_of_linkage()
else:
ValueError("Segments are available only for linkages.")
return self._segments
@property
def metric(self):
"""
Define a metric in R12 for the mechanism.
This metric is used for collision detection.
Returns
-------
AffineMetric
Metric object for the mechanism.
"""
if self._metric is None:
from .AffineMetric import AffineMetric # lazy import
mechanism_points = self.points_at_parameter(0,
inverted_part=True,
only_links=False)
self._metric = AffineMetric(self.curve(), mechanism_points)
return self._metric
@property
def linear_motions_cycle(self):
"""
A cycle of linear motions of the mechanism.
Returns
-------
list of DualQuaternion
Cycle of linear motions.
"""
if self._linear_motions_cycle is None:
# init linear motions
axes_cycle = (
self.factorizations[0].factors_with_parameter
+ self.factorizations[1].factors_with_parameter[::-1])
axes_link_cycle = []
for item in axes_cycle:
axes_link_cycle.append(DualQuaternion())
axes_link_cycle.append(item)
self._linear_motions_cycle = axes_link_cycle
return self._linear_motions_cycle
[docs]
@classmethod
def from_saved_file(cls, filename: str):
"""
Load a linkage object from a file.
Parameters
----------
filename : str
Name of the file to load the linkage object from.
Returns
-------
RationalMechanism
Loaded linkage object.
Raises
------
FileNotFoundError
If the file was not found or could not be loaded.
"""
# check if the filename has the .pkl extension
if filename[-4:] != '.pkl':
filename = filename + '.pkl'
try:
with open(filename, 'rb') as file:
mechanism = pickle.load(file)
except FileNotFoundError:
raise FileNotFoundError(f"File {filename} was not found or possible "
f"to load.")
return mechanism
[docs]
def save(self, filename: str = None):
"""
Save the linkage object to a file.
Parameters
----------
filename : str, optional
Name of the file to save the linkage object to. Default is 'saved_mechanism.pkl'.
"""
if filename is None:
filename = 'saved_mechanism.pkl'
elif filename[-4:] == '.pkl':
pass
else:
filename = filename + '.pkl'
# update the line segments (physical realization of the linkage) before saving
# self.update_segments()
with open(filename, 'wb') as file:
pickle.dump(self, file)
def _determine_tool(self, tool: Union[DualQuaternion, None, str]) -> DualQuaternion:
"""
Determine the tool frame of the mechanism.
Parameters
----------
tool : DualQuaternion, None, or str
Tool frame specification.
Returns
-------
DualQuaternion
Tool frame of the mechanism.
Raises
------
ValueError
If the tool parameter is invalid.
"""
if tool is None:
return DualQuaternion(self.evaluate(0, inverted_part=True))
elif isinstance(tool, DualQuaternion):
return tool
elif tool == 'mid_of_last_link':
# calculate the midpoint of the last link
nearly_zero = numpy.finfo(float).eps
p0 = self.factorizations[0].direct_kinematics(nearly_zero, inverted_part=True)[-1]
p1 = self.factorizations[1].direct_kinematics(nearly_zero, inverted_part=True)[-1]
# define the x axis vector - along the last link
vec_x = (p1 - p0) / numpy.linalg.norm(p1 - p0)
# get some random vector from the last joint points
vec_y_pts = self.factorizations[0].direct_kinematics(nearly_zero, inverted_part=True)[-2:]
vec_y = vec_y_pts[1] - vec_y_pts[0]
# define the z axis vector - perpendicular to the x and y vectors
vec_z = numpy.cross(vec_x, vec_y)
vec_z = vec_z / numpy.linalg.norm(vec_z)
mid = (p1 + p0) / 2
t = TransfMatrix.from_vectors(normal_x=vec_x, approach_z=vec_z, origin=mid)
return DualQuaternion(t.matrix2dq())
else:
raise ValueError("tool must be either DualQuaternion, "
"None default motion zero configuration, "
"or 'mid_of_last_link'")
[docs]
def get_design(self,
unit: str = 'rad',
scale: float = 1.0,
joint_length: float = 0.02,
washer_length: float = 0.001,
return_point_homogeneous: bool = False,
update_design: bool = False,
pretty_print: bool = True,
onshape_print: bool = False) -> tuple[numpy.ndarray, numpy.ndarray, list]:
"""
Get the design parameters of the linkage for the CAD model.
Parameters
----------
unit : str, optional
Desired unit of the angle parameters, can be 'deg' or 'rad'. Default is 'rad'.
scale : float, optional
Scale of the length parameters of the linkage. Default is 1.0.
joint_length : float, optional
Length of the joint segment in meters. Default is 0.02.
washer_length : float, optional
Length of the washer in meters. Default is 0.001.
return_point_homogeneous : bool, optional
If True, return the design points as PointHomogeneous objects. Default is False.
update_design : bool, optional
If True, update the design of the mechanism. Default is False.
pretty_print : bool, optional
If True, print the parameters in a readable form. Default is True.
onshape_print : bool, optional
If True, print the parameters in a form that can be directly copied to Onshape. Default is False.
Returns
-------
tuple of (numpy.ndarray, numpy.ndarray, list)
Design parameters of the linkage.
"""
screws = deepcopy(self.get_screw_axes())
screws.append(screws[0])
frames = self.get_frames()
connection_params = self.get_segment_connections()
mid_pts_dist = (joint_length + washer_length)
connection_params = self.map_connection_params(connection_params, mid_pts_dist)
if update_design:
# update the connection points of the joints
branch0 = connection_params[:len(self.factorizations[0].dq_axes), :]
branch1 = connection_params[len(self.factorizations[1].dq_axes):, :][::-1]
branch0 = [[pt[0], pt[1]] for pt in branch0]
# reorder point pairs of the second branch (since it goes reversed)
branch1 = [[pt[1], pt[0]] for pt in branch1]
self.factorizations[0].set_joint_connection_points_by_parameters(branch0)
self.factorizations[1].set_joint_connection_points_by_parameters(branch1)
self.update_segments()
# add the first connection point to the end of the list
connection_params = numpy.vstack((connection_params, connection_params[0, :]))
design_params = numpy.zeros((self.num_joints, 2))
for i in range(self.num_joints):
# compensate d-ith parameter of DH
design_params[i, 0] = (connection_params[i, 1]
- screws[i].get_point_param(frames[i].t))
design_params[i, 1] = (connection_params[i+1, 0]
- screws[i+1].get_point_param(frames[i+1].t))
design_points = []
for i in range(self.num_joints):
if return_point_homogeneous:
design_points.append(
[PointHomogeneous.from_3d_point(
screws[i].point_on_line(connection_params[i, 0])),
PointHomogeneous.from_3d_point(
screws[i].point_on_line(connection_params[i, 1]))])
else:
design_points.append(
[screws[i].point_on_line(connection_params[i, 0]),
screws[i].point_on_line(connection_params[i, 1])])
# ignore the first row (base frame)
dh = self.get_dh_params(unit=unit, scale=scale)
if onshape_print:
for i in range(self.num_joints):
print(f"link{i}: "
f"[{dh[i, 1]:.12f}, {dh[i, 2]:.12f}, {dh[i, 3]:.12f}], "
f"{design_params[i, 0]:.12f}, {design_params[i, 1]:.12f}")
pretty_print = False
if pretty_print:
print("Denavit-Hartenberg parameters and connection points parameters:")
for i in range(self.num_joints):
print("---")
print(f"Link {i}: d = {dh[i, 1]:.12f}, "
f"a = {dh[i, 2]:.12f}, "
f"alpha = {dh[i, 3]:.12f}")
print(f"cp_0 = {design_params[i, 0]:.12f}, "
f"cp_1 = {design_params[i, 1]:.12f}")
print("======")
print("Points which define the joint axes of mechanism in the world frame, "
"at home configuration. Note that joint0 and jointN are part of "
"the base_link joints:")
for i in range(self.num_joints):
pt0 = design_points[i][0] * scale
pt1 = design_points[i][1] * scale
print(f"Joint {i}:\n"
f" pt0 = [{pt0[0]:.12f}, {pt0[1]:.12f}, {pt0[2]:.12f}],\n"
f" pt1 = [{pt1[0]:.12f}, {pt1[1]:.12f}, {pt1[2]:.12f}]")
return dh, design_params, design_points
[docs]
def get_segment_connections(self) -> numpy.ndarray:
"""
Get the connection parameters of the linkage.
Returns
-------
numpy.ndarray
Connection points of the linkage.
"""
connection_params = numpy.zeros((self.num_joints, 2))
for i in range(len(self.factorizations[0].linkage)):
connection_params[i, :] = self.factorizations[0].linkage[i].points_params
if len(self.factorizations) > 1:
for i in range(len(self.factorizations[1].linkage)):
# iterate from back to front
connection_params[-1-i, :] = self.factorizations[1].linkage[i].points_params[::-1]
return connection_params
[docs]
def get_dh_params(self,
unit: str = 'rad',
scale: float = 1.0,
include_base: bool = False) -> numpy.ndarray:
"""
Get the standard Denavit-Hartenberg parameters of the linkage.
The parameters are in the order: theta, d, a, alpha. It follows the standard
convention. The first row is are the parameters of the base frame.
See more in the paper by :footcite:t:`Huczala2022iccma`.
.. footbibliography::
Parameters
----------
unit : str, optional
Desired unit of the angle parameters, can be 'deg', 'rad', or
'tanhalf' for tangent half-angle representation. Default is 'rad'.
scale : float, optional
Scale of the length parameters of the linkage. Default is 1.0.
include_base : bool, optional
If True, identity frame will be placed at the beginning of the list of frames.
Default is False.
Returns
-------
numpy.ndarray
Theta, d, a, alpha array of Denavit-Hartenberg parameters.
"""
frames = deepcopy(self.get_frames(include_base=include_base))
j = self.num_joints + 1 if include_base else self.num_joints
dh = numpy.zeros((j, 4))
for i in range(j):
th, d, a, al = frames[i].dh_to_other_frame(frames[i+1])
if unit == 'deg':
th = numpy.rad2deg(th)
al = numpy.rad2deg(al)
elif unit == 'tanhalf':
th = numpy.tan(th / 2)
al = numpy.tan(al / 2)
elif unit != 'rad':
raise ValueError("unit must be deg or rad")
dh[i, :] = [th, scale * d, scale * a, al]
return dh
[docs]
def get_frames(self, include_base: bool = False) -> list[TransfMatrix]:
"""
Get the frames of a linkage that follow standard Denavit-Hartenberg convention.
It returns n+2 frames, where n is the number of joints. The first frame is the
base frame, and the last frame is an updated frame of the first joint that
follows the DH convention in respect to the last joint's frame.
Parameters
----------
include_base : bool, optional
If True, identity frame will be placed at the beginning of the list of frames.
Default is False.
Returns
-------
list of TransfMatrix
List of TransfMatrix objects.
"""
from .TransfMatrix import TransfMatrix # lazy import
screws = deepcopy(self.get_screw_axes())
# add the first screw to the end of the list
screws.append(screws[0])
if include_base:
frames = [TransfMatrix()] * (self.num_joints + 2)
# insert origin as the base line
screws.insert(0, NormalizedLine())
else:
frames = [TransfMatrix()] * (self.num_joints + 1)
for i, line in enumerate(screws[1:]):
# obtain the connection points and the distance to the previous line
pts, dist, cos_angle = line.common_perpendicular_to_other_line(screws[i])
vec = pts[0] - pts[1]
if not numpy.isclose(dist, 0.0): # if the lines are skew or parallel
# normalize vec - future X axis
vec_x = vec / numpy.linalg.norm(vec)
# if parallel
if numpy.isclose(cos_angle, 1.0) or numpy.isclose(cos_angle, -1.0):
# choose origin as footpoint of the line
frames[i + 1] = TransfMatrix.from_vectors(vec_x,
line.direction,
origin=line.point_on_line())
else: # if skew
# from line.dir (future Z axis) and x create an SE3 object
frames[i+1] = TransfMatrix.from_vectors(vec_x,
line.direction,
origin=pts[0])
else: # Z axes are intersecting or coincident
if numpy.isclose(numpy.dot(frames[i].a, line.direction), 1):
# Z axes are coincident, therefore the frames are the same
frames[i+1] = deepcopy(frames[i])
elif numpy.isclose(numpy.dot(frames[i].a, line.direction), -1):
# Z axes are coincident, but differ in orientation
rot_x_pi = TransfMatrix.from_rpy([numpy.pi, 0, 0])
frames[i + 1] = TransfMatrix(frames[i].matrix @ rot_x_pi.matrix)
else: # Z axis intersect with an angle
# future X axis as cross product of previous Z axis and new Z axis
vec_x = numpy.cross(frames[i].a, line.direction)
frames[i + 1] = TransfMatrix.from_vectors(vec_x,
line.direction,
origin=pts[0])
if not include_base:
# update the last frame to close the linkage loop
frames[0] = frames[-1]
return frames
[docs]
def get_global_frames(self) -> list[TransfMatrix]:
"""
Get the frames of the linkage in the global coordinate system.
Returns
-------
list of TransfMatrix
List of TransfMatrix objects.
"""
local_frames = self.get_frames(include_base=True)[1:]
global_frames = [TransfMatrix()]
for i in range(1, len(local_frames)):
global_frames.append(global_frames[i-1] * local_frames[i])
return global_frames
[docs]
def get_screw_axes(self) -> list[NormalizedLine]:
"""
Get the normalized lines (screw axes, Plucker coordinates) of the linkage.
The lines are in home configuration. They consist of two factorizations, and
the second factorization axes must be reversed.
Returns
-------
list of NormalizedLine
List of NormalizedLine objects.
"""
screws = []
for axis in self.factorizations[0].dq_axes:
screws.append(NormalizedLine(axis.dq2screw()))
branch2 = []
for axis in self.factorizations[1].dq_axes:
branch2.append(NormalizedLine(axis.dq2screw()))
return screws + branch2[::-1]
[docs]
def map_connection_params(self,
connection_params: numpy.ndarray,
midpoints_distance: float) -> numpy.ndarray:
"""
Map the connection parameters to the given joint length.
Parameters
----------
connection_params : numpy.ndarray
List of connection points parameters of the linkage.
midpoints_distance : float
Distance between the midpoints of the two links that connect at a joint.
Returns
-------
numpy.ndarray
Mapped connection points parameters.
"""
c_params = numpy.asarray(connection_params)
for i in range(len(c_params)):
c_params_len = numpy.linalg.norm(c_params[i, 0] - c_params[i, 1])
if not numpy.allclose(c_params_len, midpoints_distance):
c_params[i, :] = self._map_joint_segment(c_params[i, :],
midpoints_distance)
return c_params
@staticmethod
def _map_joint_segment(points_params: numpy.ndarray, midpoints_distance: float):
"""
Map the joint segment to the scale of the linkage.
Parameters
----------
points_params : numpy.ndarray
List of connection points parameters of the joint segment.
midpoints_distance : float
Distance between the midpoints of the two links that connect at a joint.
Returns
-------
numpy.ndarray
Mapped joint segment.
"""
def map_interval(input_interval, max_length):
mid_point = (input_interval[0] + input_interval[1]) / 2
if input_interval[0] < input_interval[1]:
mapped_interval = [mid_point - max_length/2, mid_point + max_length/2]
else:
mapped_interval = [mid_point + max_length/2, mid_point - max_length/2]
return mapped_interval
points_params = numpy.asarray(points_params)
new_params = map_interval(points_params, midpoints_distance)
# Calculate midpoints
#midpoint1 = new_params[0] + (new_params[1] - new_params[0]) / 4
#midpoint2 = new_params[0] + 3 * (new_params[1] - new_params[0]) / 4
return new_params
[docs]
def collision_check(self,
parallel: bool = False,
pretty_print: bool = True,
only_links: bool = False,
terminate_on_first: bool = False):
"""
Perform full-cycle collision check on the line-model linkage.
By default, the collision check is performed in non-parallel mode. This is
faster for 4-bar linkages and 6-bar lingakes with a "simpler" motion curve,
but slower for 6-bar linkages with "complex" motions.
Parameters
----------
parallel : bool, optional
If True, perform collision check in parallel using multiprocessing. Default is False.
pretty_print : bool, optional
If True, print the results in a readable form. Default is True.
only_links : bool, optional
If True, only link-link collisions are checked. Default is False.
terminate_on_first : bool, optional
If True, terminate the collision check when the first collision is found. Default is False.
Returns
-------
list of float
List of collision check colliding parameter values.
"""
start_time = time()
print("Collision check started...")
# update the line segments (physical realization of the linkage)
self.update_segments()
iters = []
# iterate over all line segments
for ii in range(len(self.segments)):
# for each line segment, iterate over all other line segments that are not
# its immediate neighbors
for jj in range(ii + 2, len(self.segments)):
# in only links should be checked (joint segments have minimal length)
if only_links:
if (self.segments[ii].type == 'j'
or self.segments[jj].type == 'j'
or jj - ii == 2): # skip neighbouring links
pass
else:
iters.append((ii, jj))
else:
iters.append((ii, jj))
# remove the last neighbouring pair of links
if only_links:
# find the pair with the highest difference
max_pair = max(iters, key=lambda x: x[1] - x[0])
# remove the pair from the list
iters.remove(max_pair)
else: # remove the first link and last joint segments anyway (neighbours)
iters.remove((0, len(self.segments) - 1))
print(f"--- number of tasks to solve: {len(iters)} ---")
if parallel:
collision_results = self._collision_check_parallel(iters)
else:
collision_results = self._collision_check_nonparallel(iters,
terminate_on_first)
results = [r for r in collision_results if r is not None]
flattened_results = [item for sublist in results for item in sublist]
if len(results) == 0:
flattened_results = None
end_time = time()
print(f"--- collision check finished in {end_time - start_time} seconds.")
if pretty_print:
if flattened_results is None:
print("No collisions found.")
else:
print(f"The linkage is colliding {len(flattened_results)} times at the "
f"following parameter values:")
for res in flattened_results:
print(res)
return flattened_results
def _collision_check_parallel(self, iters: list[tuple[int, int]]):
"""
Perform collision check in parallel using multiprocessing.
Slower for 4-bar linkages and 6-bar lingakes with a "simpler" motion curve,
faster for 6-bar linkages with "complex" motions.
Parameters
----------
iters : list of tuple[int, int]
List of tuples of indices of the line segments to be checked.
Returns
-------
list of collision check results
List of collision check results.
"""
print("--- running in parallel ---")
import concurrent.futures
with concurrent.futures.ProcessPoolExecutor() as executor:
results = executor.map(self._check_given_pair, iters)
return list(results)
def _collision_check_nonparallel(self, iters: list[tuple[int, int]],
terminate_on_first: bool = False):
"""
Perform collision check in non-parallel mode.
Default option. Faster for 4-bar linkages and 6-bar lingakes with a "simpler"
motion curve, slower for 6-bar linkages with "complex" motions.
Parameters
----------
iters : list of tuple[int, int]
List of tuples of indices of the line segments to be checked.
terminate_on_first : bool, optional
If True, terminate the collision check when the first collision is found. Default is False.
Returns
-------
list of collision check results
List of collision check results.
"""
results = []
for val in iters:
collsion = self._check_given_pair(val)
results.append(collsion)
if terminate_on_first and collsion is not None:
break
return results
def _check_given_pair(self, iters: tuple[int, int]) -> list[float]:
"""
Perform collision check for a given pair of line segments and evaluate it.
Parameters
----------
iters : tuple[int, int]
Tuple of indices of the line segments to be checked.
Returns
-------
list of float
Collision check result.
"""
i = iters[0]
j = iters[1]
# check if two lines are colliding
collisions, coll_pts = self.colliding_lines(self.segments[i].equation,
self.segments[j].equation)
if collisions is not None:
# check if the collision is between the physical line segments
physical_collision = [
self.segments[i].is_point_in_segment(coll_pts[k], t_val) and
self.segments[j].is_point_in_segment(coll_pts[k], t_val)
for k, t_val in enumerate(collisions)
]
else:
physical_collision = [False]
result = []
if True in physical_collision:
for i, res in enumerate(physical_collision):
if res:
result.append(collisions[i])
#result = f"{physical_collision} at parameters: {collisions} for linkage pair: {self.segments[i].type}_{self.segments[i].factorization_idx}{self.segments[i].idx} X {self.segments[j].type}_{self.segments[j].factorization_idx}{self.segments[j].idx}"
else:
#result = f"no collision between {self.segments[i].type}_{self.segments[i].factorization_idx}{self.segments[i].idx} X {self.segments[j].type}_{self.segments[j].factorization_idx}{self.segments[j].idx}"
result = None
return result
[docs]
def colliding_lines(self, l0: NormalizedLine, l1: NormalizedLine
) -> tuple[list[float], list[PointHomogeneous]]:
"""
Return the lines that are colliding in the linkage.
Parameters
----------
l0 : NormalizedLine
Equation of the first line.
l1 : NormalizedLine
Equation of the second line.
Returns
-------
tuple of (list of t values, list of intersection points)
Tuple containing a list of t values and a list of intersection points.
"""
t = sympy.Symbol("t")
# lines are colliding when expr = 0
expr = numpy.dot(l0.direction, l1.moment) + numpy.dot(l0.moment, l1.direction)
# reparametrize the expresion by t -> (t + 1) / 2 to interval [-1, 1]
e = sympy.Expr(expr).subs(t, (t + 1) / 2).evalf()
expr_poly = sympy.Poly(e.args[0], t)
expr_coeffs = expr_poly.all_coeffs()
# convert to numpy polynomial
expr_n = numpy.array(expr_coeffs, dtype="float64")
np_poly = numpy.polynomial.polynomial.Polynomial(expr_n[::-1])
# inversing coeffs enables to solve intervals (-oo, 0) and (0, oo), that are
# actually mapped to [-1, 1]
np_poly_inversed = numpy.polynomial.polynomial.Polynomial(expr_n)
# solve for t
colliding_lines_sol = np_poly.roots()
colliding_lines_sol_inversed = np_poly_inversed.roots()
# extract real solutions
sol_real = colliding_lines_sol.real[numpy.isclose(colliding_lines_sol.imag,
0, atol=1e-5)]
sol_real_inversed = colliding_lines_sol_inversed.real[
numpy.isclose(colliding_lines_sol_inversed.imag, 0, atol=1e-5)]
sol_real = [((sol + 1)/2) for sol in sol_real]
sol_real_inversed = [((sol + 1)/2) for sol in sol_real_inversed]
solutions = deepcopy(sol_real)
for sol in sol_real_inversed:
if numpy.isclose(sol, 0):
# eps is very small number (avoid division by zero)
sol = 1 / numpy.finfo(float).eps
else:
sol = 1 / sol
solutions = numpy.append(solutions, sol)
intersection_points = self.get_intersection_points(l0, l1, solutions)
return solutions, intersection_points
[docs]
@staticmethod
def get_intersection_points(l0: NormalizedLine, l1: NormalizedLine,
t_params: list[float]):
"""
Return the intersection points of two lines.
Parameters
----------
l0 : NormalizedLine
Equation of the first line.
l1 : NormalizedLine
Equation of the second line.
t_params : list[float]
List of parameter values - points of intersection.
Returns
-------
list of PointHomogeneous
List of intersection points.
"""
intersection_points = [PointHomogeneous()] * len(t_params)
for i, t_val in enumerate(t_params):
# evaluate the lines at the given parameter
l0e = l0.evaluate(t_val).evalf()
l1e = l1.evaluate(t_val).evalf()
# common perpendicular to the two lines - there is none since they
# intersect, therefore from the list of two points only 1 is needed
inters_points, d, c = l0e.common_perpendicular_to_other_line(l1e)
intersection_points[i] = PointHomogeneous.from_3d_point(inters_points[0])
return intersection_points
def _get_line_segments_of_linkage_old(self) -> list:
"""
Return the line segments of the linkage.
Line segments are the physical realization of the linkage. This method obtains
their motion equations using default connection points of the factorizations
(default meaning the static points in the home configuration).
Returns
-------
list of LineSegment
List of LineSegment objects.
"""
t = sympy.Symbol("t")
segments = [[], []]
# base (static) link has index 0 in the list of the 1st factorization
eq, p0, p1 = self.factorizations[0].base_link(
self.factorizations[1].linkage[0].points[0])
segments[0].append(LineSegment(eq, p0, p1, linkage_type="l", f_idx=0, idx=0))
# static joints
segments[0].append(LineSegment(*self.factorizations[0].joint(0),
linkage_type="j", f_idx=0, idx=0))
segments[1].append(LineSegment(*self.factorizations[1].joint(0),
linkage_type="j", f_idx=1, idx=0))
# moving links and joints
for i in range(2):
for j in range(1, self.factorizations[i].number_of_factors):
line, p0, p1 = self.factorizations[i].link(j)
link = self.factorizations[i].act(line, end_idx=j-1, param=t)
p0 = self.factorizations[i].act(p0, end_idx=j-1, param=t)
p1 = self.factorizations[i].act(p1, end_idx=j-1, param=t)
segments[i].append(LineSegment(link, p0, p1, default_line=line,
linkage_type="l", f_idx=i, idx=j))
line, p0, p1 = self.factorizations[i].joint(j)
joint = self.factorizations[i].act(line, end_idx=j, param=t)
p0 = self.factorizations[i].act(p0, end_idx=j, param=t)
p1 = self.factorizations[i].act(p1, end_idx=j, param=t)
segments[i].append(LineSegment(joint, p0, p1, default_line=line,
linkage_type="j", f_idx=i, idx=j))
# tool (moving - acted) link has index -1 in the list of the 2nd factorization
tool_link_line, p0, p1 = self.factorizations[0].tool_link(
self.factorizations[1].linkage[-1].points[1])
tool_link = self.factorizations[0].act(tool_link_line, param=t)
p0 = self.factorizations[0].act(p0, param=t)
p1 = self.factorizations[1].act(p1, param=t)
tool_idx = self.factorizations[1].number_of_factors
segments[1].append(LineSegment(tool_link, p0, p1, default_line=tool_link_line,
linkage_type="l", f_idx=1, idx=tool_idx))
return segments[0] + segments[1][::-1]
def _get_line_segments_of_linkage(self) -> list:
"""
Return the line segments of the linkage.
Line segments are the physical realization of the linkage. This method obtains
their motion equations using default connection points of the factorizations
(default meaning the static points in the home configuration).
Returns
-------
list of LineSegment
List of LineSegment objects.
"""
t = sympy.Symbol("t")
segments = []
# base (static) link has index 0 in the list of the 1st factorization
eq, p0, p1 = self.factorizations[0].base_link(
self.factorizations[1].linkage[0].points[0])
segments.append(LineSegment(eq, p0, p1, linkage_type="l", f_idx=0, idx=0))
# static joints
segments.append(LineSegment(*self.factorizations[0].joint(0),
linkage_type="j", f_idx=0, idx=0))
# moving links and joints
i = 0
for j in range(1, self.factorizations[i].number_of_factors):
line, p0, p1 = self.factorizations[i].link(j)
link = self.factorizations[i].act(line, end_idx=j-1, param=t)
p0 = self.factorizations[i].act(p0, end_idx=j-1, param=t)
p1 = self.factorizations[i].act(p1, end_idx=j-1, param=t)
segments.append(LineSegment(link, p0, p1, default_line=line,
linkage_type="l", f_idx=i, idx=j))
line, p0, p1 = self.factorizations[i].joint(j)
joint = self.factorizations[i].act(line, end_idx=j, param=t)
p0 = self.factorizations[i].act(p0, end_idx=j, param=t)
p1 = self.factorizations[i].act(p1, end_idx=j, param=t)
segments.append(LineSegment(joint, p0, p1, default_line=line,
linkage_type="j", f_idx=i, idx=j))
# tool (moving - acted) link has index -1 in the list of the 2nd factorization
tool_link_line, p0, p1 = self.factorizations[0].tool_link(
self.factorizations[1].linkage[-1].points[1])
tool_link = self.factorizations[0].act(tool_link_line, param=t)
p0 = self.factorizations[0].act(p0, param=t)
p1 = self.factorizations[1].act(p1, param=t)
tool_idx = self.factorizations[1].number_of_factors
segments.append(LineSegment(tool_link, p0, p1, default_line=tool_link_line,
linkage_type="l", f_idx=1, idx=tool_idx))
i = 1
for j in range(self.factorizations[i].number_of_factors -1, 0, -1):
line, p0, p1 = self.factorizations[i].joint(j)
joint = self.factorizations[i].act(line, end_idx=j, param=t)
p0 = self.factorizations[i].act(p0, end_idx=j, param=t)
p1 = self.factorizations[i].act(p1, end_idx=j, param=t)
segments.append(LineSegment(joint, p0, p1, default_line=line,
linkage_type="j", f_idx=i, idx=j))
line, p0, p1 = self.factorizations[i].link(j)
link = self.factorizations[i].act(line, end_idx=j-1, param=t)
p0 = self.factorizations[i].act(p0, end_idx=j-1, param=t)
p1 = self.factorizations[i].act(p1, end_idx=j-1, param=t)
segments.append(LineSegment(link, p0, p1, default_line=line,
linkage_type="l", f_idx=i, idx=j))
segments.append(LineSegment(*self.factorizations[1].joint(0),
linkage_type="j", f_idx=1, idx=0))
return segments
[docs]
def get_motion_curve(self):
"""
Return the rational motion curve of the linkage as RationalCurve object.
Returns
-------
RationalCurve
Motion curve of the linkage.
"""
return self.curve()
[docs]
def singularity_check(self):
"""
Perform singularity check of the mechanism.
Returns
-------
list
List of singularities.
"""
from .SingularityAnalysis import SingularityAnalysis # lazy import
sa = SingularityAnalysis()
return sa.check_singularity(self)
[docs]
def smallest_polyline(self, update_design: bool = False) -> tuple[list, list, float]:
"""
Obtain the smallest polyline links for the mechanism.
Parameters
----------
update_design : bool, optional
If True, update also the design of the mechanism. Default is False.
Returns
-------
tuple of (list, list, float)
List of points of the smallest polyline, list of points parameters,
result of the optimization.
"""
from .CollisionFreeOptimization import CollisionFreeOptimization # lazy import
# get smallest polyline
pts, points_params, res = CollisionFreeOptimization(self).smallest_polyline()
# update the design of the mechanism
if update_design:
if len(self.factorizations) == 1:
self.factorizations[0].set_joint_connection_points_by_parameters(points_params)
else:
self.factorizations[0].set_joint_connection_points_by_parameters(
points_params[:len(self.factorizations[0].dq_axes)])
self.factorizations[1].set_joint_connection_points_by_parameters(
points_params[len(self.factorizations[1].dq_axes):][::-1])
return pts, points_params, res
[docs]
def collision_free_optimization(self,
method: str = None,
step_length=25,
min_joint_segment_length: float = 0.001,
max_iters: int = 10,
**kwargs):
"""
Perform collision-free optimization of the mechanism.
Parameters
----------
method : str, optional
Method of optimization, can be 'combinatorial_search' by
:footcite:t:`Li2020`. Default is None.
step_length : float, optional
Length of the step, i.e. the shift distance value. Default is 25.
min_joint_segment_length : float, optional
Minimum length of the joint segment. Default is 0.001.
max_iters : int, optional
Maximum number of iterations. Default is 10.
**kwargs
Additional keyword arguments for the optimization method.
Returns
-------
list
List of collision-free points parameters.
"""
from .CollisionFreeOptimization import CollisionFreeOptimization
optimizer = CollisionFreeOptimization(self)
if method is None:
method = 'combinatorial_search'
match method:
case 'combinatorial_search':
results = optimizer.optimize(method=method,
step_length=step_length,
min_joint_segment_length=min_joint_segment_length,
max_iters=max_iters,
**kwargs)
case _:
raise ValueError("Invalid method.")
return results
[docs]
def points_at_parameter(self,
t_param: float,
inverted_part: bool = False,
only_links: bool = False) -> list[PointHomogeneous]:
"""
Get the points of the mechanism at the given parameter.
Parameters
----------
t_param : float
Parameter value.
inverted_part : bool, optional
If True, return the evaluated points for the inverted part of the mechanism.
Default is False.
only_links : bool, optional
If True, instead of two points per joint segment, return only the first one.
Default is False.
Returns
-------
list of PointHomogeneous
List of connection points of the mechanism.
"""
branch0 = self.factorizations[0].direct_kinematics(t_param,
inverted_part=inverted_part)
branch1 = self.factorizations[1].direct_kinematics(t_param,
inverted_part=inverted_part)
points = branch0 + branch1[::-1]
if only_links:
points = [points[i] for i in range(0, len(points), 2)]
return [PointHomogeneous.from_3d_point(p) for p in points]
[docs]
def forward_kinematics(self,
joint_angle: float,
unit: str = 'rad') -> DualQuaternion:
"""
Calculate forward (direct) kinematics of the mechanism. Radians are default.
Parameters
----------
joint_angle : float
Angle of the joint.
unit : str, optional
Unit of the joint angle, can be 'rad' or 'deg'. Default is 'rad'.
Returns
-------
DualQuaternion
Tool frame of the mechanism.
"""
if unit == 'deg':
joint_angle = numpy.deg2rad(joint_angle)
elif unit != 'rad':
raise ValueError("unit must be deg or rad")
t = self.factorizations[0].joint_angle_to_t_param(joint_angle)
return DualQuaternion(self.evaluate(t)) * self.tool_frame
[docs]
def direct_kinematics(self,
joint_angle: float,
unit: str = 'rad') -> DualQuaternion:
"""
Calculate direct (forward) kinematics of the mechanism. Radians are default.
Calls the forward_kinematics method.
Parameters
----------
joint_angle : float
Angle of the joint.
unit : str, optional
Unit of the joint angle, can be 'rad' or 'deg'. Default is 'rad'.
Returns
-------
DualQuaternion
Tool frame of the mechanism.
"""
return self.forward_kinematics(joint_angle, unit)
[docs]
def inverse_kinematics(self,
pose: Union[DualQuaternion, TransfMatrix],
unit: str = 'rad',
method: str = 'gauss-newton',
robust: bool = False
) -> float:
"""
Calculate inverse kinematics for given pose. Returns the joint angle in radians.
Parameters
----------
pose : Union[DualQuaternion, TransfMatrix]
Pose of the mechanism.
unit : str, optional
Unit of the joint angle, can be 'rad', 'deg', or 't' as
t is the parameter of the motion curve. Default is 'rad'.
method : str, optional
Numerically for 'gauss-newton' or 'algebraic'; 'algebraic'
requires the input pose to be "achievable" by the mechanism, i.e. the pose
must be on Study quadric and the mechanism must be able to reach it.
robust : bool, optional
If True, use the Gauss-Newton method with
many initial guesses and more iteration steps. Default is False.
Returns
-------
float
Joint angle in radians or degrees.
"""
if isinstance(pose, TransfMatrix):
pose = DualQuaternion(pose.matrix2dq())
elif not isinstance(pose, DualQuaternion):
raise ValueError("pose must be either DualQuaternion or TransfMatrix")
if unit not in {'rad', 'deg', 't'}:
raise ValueError("unit must be deg or rad")
if method == 'algebraic':
raise NotImplementedError("Algebraic method is not implemented yet.")
elif method == 'gauss-newton':
t = self._ik_gauss_newton(pose, robust_search=robust)
else:
raise ValueError("method must be either 'algebraic' or 'gauss-newton")
if unit == 't':
return t
else:
joint_angle = self.factorizations[0].t_param_to_joint_angle(t)
if unit == 'deg':
joint_angle = numpy.rad2deg(joint_angle)
return joint_angle
def _ik_gauss_newton(self,
goal_pose: DualQuaternion,
robust_search: bool = False) -> float:
"""
Calculate inverse kinematics using Gauss-Newton method.
Parameters
----------
goal_pose : DualQuaternion
Pose of the mechanism.
robust_search : bool, optional
If True, use many initial guesses. Default is False.
Returns
-------
float
Parameter value.
Warns
-----
ConvergenceWarning
If the method does not converge.
"""
def run_gauss_newton(pose, robust):
t = sympy.Symbol("t")
curves = [self.curve(), self.curve().inverse_curve()]
success = False
inversed_part = False
t_min = [None, float('inf')]
t_init_set = [0., -0.999999999, 0.999999999, -0.5, 0.5]
t_res = None
max_iterations = 10
tol = 1e-10
# map pose to the motion curve of identity frame
pose = pose * self.tool_frame.inv()
if robust:
t_init_set = numpy.linspace(-1.0, 1.0, 30)
max_iterations = 50
tol = 1e-15
for inv, curve in enumerate(curves):
if inv == 1:
inversed_part = True
norm_curve = [element / curve.set_of_polynomials[0]
for element in curve.set_of_polynomials]
c_diff = [element.diff(t) for element in norm_curve]
# numerical preparation of the derivatives
c_diff_funcs = [sympy.lambdify(t, expr, modules='numpy')
for expr in c_diff]
def c_diff_lambdified(x: float):
return numpy.array([f(x) for f in c_diff_funcs])
curve_funcs = [sympy.lambdify(t, expr, modules='numpy')
for expr in curve.symbolic]
def curve_lambdified(x: float):
return numpy.array([f(x) for f in curve_funcs])
for t_val in t_init_set:
step_size = 1.0
previous_error = float('inf')
for i in range(max_iterations):
# check if t_val is valid, i.e. must be in the range [-1, 1]
if (t_val == sympy.nan or numpy.isnan(t_val) or t_val > 10.0
or t_val < -10.0):
break
target_pose = pose.array()
current_pose = curve_lambdified(t_val)
c_diff_eval = c_diff_lambdified(t_val)
# error to desired pose
if (numpy.isclose(target_pose[0], 0.0)
or numpy.isclose(current_pose[0], 0.0)):
twist_to_desired = target_pose - current_pose
else:
twist_to_desired = (target_pose / target_pose[0]
- current_pose / current_pose[0])
square_dist_to_desired = numpy.sum(twist_to_desired ** 2)
t_val += (step_size * (c_diff_eval @ twist_to_desired)
/ numpy.sum(c_diff_eval ** 2))
if square_dist_to_desired > previous_error:
step_size *= 0.5
else:
step_size = 1.0
if square_dist_to_desired < tol:
success = True
t_res = t_val
break
if square_dist_to_desired < t_min[1]:
t_min = [t_val, square_dist_to_desired]
if success:
break
if success:
break
if not success:
t_res = t_min[0]
if inversed_part:
if numpy.isclose(t_res, 0.0):
t_res = numpy.finfo(numpy.float64).tiny
t_res = 1 / t_res
return t_res, success
t_res, success = run_gauss_newton(pose=goal_pose, robust=robust_search)
if not success:
print("Fast search did not converge. Retrying with different initial "
"guesses...")
t_res, success = run_gauss_newton(pose=goal_pose, robust=True)
print("...done.")
print("Converged successfully.") if success else (
warn("Not converged, providing the closest result."))
return t_res
[docs]
@staticmethod
def traj_p2p_joint_space(joint_angle_start: float,
joint_angle_end: float,
velocity_start: float = 0.0,
velocity_end: float = 0.0,
unit: str = 'rad',
time_sec: float = 1.0,
num_points: int = 100,
method: str = 'quintic',
generate_csv: bool = False) -> tuple:
"""
Generate point to point straight line joint space trajectory.
This method originates from book Modern Robotics :footcite:p:`Lynch2017`
by Kevin M. Lynch
and Frank C. Park, and related software package published under MIT
licence and available at: https://github.com/NxRLab/ModernRobotics
.. footbibliography::
Parameters
----------
joint_angle_start : float
Start parameter value.
joint_angle_end : float
End parameter value.
velocity_start : float, optional
Start velocity. Default is 0.0.
velocity_end : float, optional
End velocity. Default is 0.0.
unit : str, optional
Unit of the joint angle, can be 'rad' or 'deg'. Default is 'rad'.
time_sec : float, optional
Time of the trajectory in seconds. Default is 1.0.
num_points : int, optional
Number of discrete points in the trajectory. Default is 100.
method : str, optional
Method of trajectory generation, can be 'quintic' or 'cubic'. Default is 'quintic'.
generate_csv : bool, optional
If True, generate a CSV file with the trajectory. Default is False.
Returns
-------
tuple
Tuple of joint position (angle), velocity, and acceleration.
Examples
--------
.. code-block:: python
from rational_linkages import RationalCurve, RationalMechanism
import numpy
import matplotlib.pyplot as plt
coeffs = numpy.array([[0, 0, 0],
[4440, 39870, 22134],
[16428, 9927, -42966],
[-37296,-73843,-115878],
[0, 0, 0],
[-1332, -14586, -7812],
[-2664, -1473, 6510],
[-1332, -1881, -3906]])
c = RationalCurve.from_coeffs(coeffs)
m = RationalMechanism(c.factorize())
time = 3 # seconds
n_steps = 100
t0 = 0
t1 = numpy.pi/4
method = 'quintic'
#method = 'cubic'
pos, vel, acc = m.traj_p2p_joint_space(joint_angle_start=t0,
joint_angle_end=t1,
time_sec=time,
method=method,
num_points=n_steps)
# plot the trajectory
plt.plot(pos)
plt.plot(vel)
plt.plot(acc)
plt.xlabel('Time [sec]')
plt.legend(['Position [rad]', 'Velocity [rad/s]', 'Acceleration [rad/s^2]'])
plt.grid()
plt.show()
.. clear-namespace::
"""
if unit == 'deg':
joint_angle_start = numpy.deg2rad(joint_angle_start)
joint_angle_end = numpy.deg2rad(joint_angle_end)
elif unit != 'rad':
raise ValueError("unit must be deg or rad")
def cubic_time_scaling(tot_time, step):
return 3 * (step / tot_time) ** 2 - 2 * (step / tot_time) ** 3
def quintic_time_scaling(tot_time, step):
return (10 * (step / tot_time) ** 3
- 15 * (step / tot_time) ** 4
+ 6 * (step / tot_time) ** 5)
def quintic_time_scaling_with_velocity(t, tot_time, th_0, th_f, v_0, v_f):
t3, t4, t5 = tot_time ** 3, tot_time ** 4, tot_time ** 5
a_0, a_1, a_2 = th_0, v_0, 0
a_3 = (20 * (th_f - th_0) - (8 * v_f + 12 * v_0) * tot_time) / (2 * t3)
a_4 = (30 * (th_0 - th_f) + (14 * v_f + 16 * v_0) * tot_time) / (2 * t4)
a_5 = (12 * (th_f - th_0) - (6 * v_f + 6 * v_0) * tot_time) / (2 * t5)
return (a_0 + a_1 * t + a_2 * t ** 2 + a_3 * t ** 3
+ a_4 * t ** 4 + a_5 * t ** 5)
time_gap = time_sec / num_points
traj = numpy.zeros(num_points)
for i in range(num_points):
if method == 'cubic' or method == 'quintic':
if method == 'cubic':
scaling = cubic_time_scaling(time_sec, time_gap * i)
elif method == 'quintic':
scaling = quintic_time_scaling(time_sec, time_gap * i)
traj[i] = (scaling * numpy.array(joint_angle_end)
+ (1 - scaling) * numpy.array(joint_angle_start))
elif method == 'quintic_with_velocity':
traj[i] = quintic_time_scaling_with_velocity(time_gap * i,
time_sec,
joint_angle_start,
joint_angle_end,
velocity_start,
velocity_end)
else:
raise ValueError("method must be either 'cubic', 'quintic', "
"or 'quintic_with_velocity'")
if generate_csv:
RationalMechanism._generate_csv(traj, time_gap)
vel = numpy.diff(traj, axis=0) * num_points / time_sec
acc = numpy.diff(vel, axis=0) * num_points / time_sec
return traj, vel, acc
@staticmethod
def _generate_csv(traj, time_gap):
"""
Generate a CSV file with the trajectory.
Parameters
----------
traj
Trajectory.
time_gap
Time gap.
"""
time_space = numpy.arange(0, len(traj) * time_gap, time_gap)
pos = traj
vel = numpy.diff(traj, axis=0) / time_gap
#vel = numpy.append(numpy.array([0.0]), vel) # add .0 to equalize the array length
vel = numpy.append(vel, vel[-1])
acc = numpy.diff(vel, axis=0) / time_gap
#acc = numpy.append(acc, numpy.array([0.0])) # add .0 to equalize the array length
acc = numpy.append(acc, acc[-1])
# Stack the arrays horizontally to create a 2D array with 4 columns
data = numpy.column_stack((time_space, pos, vel, acc))
# Save the stacked array to a CSV file
numpy.savetxt('trajectory.csv', data, delimiter=',', fmt='%1.6f')
[docs]
def update_metric(self):
"""
Update the metric of the mechanism.
Set to none so that the metric is recalculated when needed.
"""
self._metric = None
[docs]
def update_segments(self):
"""
Update the line segments of the linkage.
"""
self._segments = None
LineSegment.reset_counter()
self._segments = self._get_line_segments_of_linkage()
[docs]
def relative_motion(self,
static: int,
moving: int) -> DualQuaternion:
"""
Calculate the relative motion between given pair of links or joints.
The method checks if the relative motion between the two links or joints
already exists in the self.relative_motions attribute. If it does, the method
returns the relative motion. If it does not, the method calculates the relative
motion and adds it to the self.relative_motions attribute.
Parameters
----------
static : int
Index of the static link or joint.
moving : int
Index of the moving link or joint.
Returns
-------
DualQuaternion
Relative motion between the two links or joints.
"""
if static == moving:
raise ValueError("static and moving cannot be the same")
motion_cycle = self._shortest_path(static, moving)
rel_motion = DualQuaternion()
for idx in motion_cycle:
rel_motion *= self.linear_motions_cycle[idx]
return rel_motion
def _shortest_path(self, start: int, end: int) -> list[int]:
"""
Return the shortest circular slice of `path` from index `start` to `end`.
If going “forward” (increasing index modulo n) is shorter than
going “backward”, you get the forward slice; otherwise the backward slice.
Parameters
----------
start : int
Start index.
end : int
End index.
Returns
-------
list[int]
List of indices of the shortest path.
"""
path = list(range(2*self.num_joints))
n = len(path)
# distance going forward (wrapping at n)
dist_fwd = (end - start) % n
# distance going backward
dist_bwd = (start - end) % n
if dist_fwd <= dist_bwd:
# walk forward dist_fwd steps, including the start (0) up to end
motion_indices = [path[(start + i) % n] for i in range(dist_fwd + 1)]
else:
# walk backward dist_bwd steps
motion_indices = [path[(start - i) % n] for i in range(dist_bwd + 1)]
# slice out the last element
return motion_indices[:-1]
[docs]
def get_design_points(self, scale: float = 1.0) -> numpy.ndarray:
"""
Return the design points of the mechanism, scaled and closed as a loop.
Parameters
----------
scale : float, optional
Scaling factor for the points. Default is 1.0.
Returns
-------
numpy.ndarray
Points array.
"""
_, _, pts = self.get_design(pretty_print=False, update_design=True)
pts = numpy.vstack(pts) * scale
pts = numpy.vstack([pts, pts[0]])
return pts
[docs]
def export_single_mesh(self,
scale: float = 1.0,
link_diameter: float = 0.01,
joint_diameter: float = 0.02,
add_tool_frame: bool = True,
file_name: str = 'mechanism.stl') -> None:
"""
Export a single STL mesh of the mechanism at home configuration.
Parameters
----------
scale : float, optional
Scaling factor of the mechanism. Default is 1.0.
link_diameter : float, optional
Radius of the link cylinders. Default is 0.01.
joint_diameter : float, optional
Radius of the joint cylinders. Default is 0.02.
add_tool_frame : bool, optional
If True, add a tool link with frame representing the tool frame. Default is True.
file_name : str, optional
Name of the output STL file. Default is 'mechanism.stl'.
"""
from rational_linkages.LinkageCAD import LinkageCAD # lazy import
design_pts = self.get_design_points(scale=scale)
LinkageCAD(design_points=design_pts).export_single_mesh(
link_diameter=link_diameter,
joint_diameter=joint_diameter,
add_tool_frame=add_tool_frame,
file_name=file_name
)
[docs]
def export_single_solid(
self,
units: str = 'mm',
scale: float = 1.0,
link_diameter: float = 10,
joint_diameter: float = 20,
add_tool_frame: bool = True,
file_name: str = "mechanism.step") -> None:
"""
Export a single CAD solid (STEP) of the mechanism at home configuration.
Parameters
----------
units : str, optional
Unit of the scale, can be 'm' or 'mm'. Default is 'mm'.
scale : float, optional
Scaling factor. Default is 1.0.
link_diameter : float, optional
Diameter of link cylinders. Default is 10.
joint_diameter : float, optional
Diameter of joint cylinders. Default is 20.
add_tool_frame : bool, optional
If True, add tool frame geometry. Default is True.
file_name : str, optional
Output file name (.step recommended). Default is "mechanism.step".
"""
from rational_linkages.LinkageCAD import LinkageCAD # lazy import
design_pts = self.get_design_points(scale=scale)
LinkageCAD(design_points=design_pts).export_single_solid(
units=units,
link_diameter=link_diameter,
joint_diameter=joint_diameter,
add_tool_frame=add_tool_frame,
file_name=file_name
)
[docs]
def export_solids(self,
units: str = "mm",
link_diameter
: float = 10,
joint_diameter: float = 20,
add_tool_frame: bool = True,
file_name: str = "mechanism_parts.step"
):
"""
Export mechanism assembly with individual CAD solids (STEP).
Parameters
----------
units : str, optional
Units for the design (e.g., "mm" or "m"). Default is "mm".
link_diameter : float, optional
Diameter of the cylindrical links (default 10; i.e. mm). Default is 10.
joint_diameter : float, optional
Diameter of the cylindrical joints (default 20; i.e. mm). Default is 20.
add_tool_frame : bool, optional
Whether to include a simple tool frame geometry. Default is True.
file_name : str, optional
Output STEP file name. Default is "mechanism_parts.step".
"""
from rational_linkages.LinkageCAD import LinkageCAD # lazy import
design_pts = self.get_design_points(scale=1.0)
LinkageCAD(design_points=design_pts).export_solids(
units=units,
link_diameter=link_diameter,
joint_diameter=joint_diameter,
add_tool_frame=add_tool_frame,
file_name=file_name
)