def visualize_urscript(script):
M = [
[-1000, 0, 0, 0],
[0, 1000, 0, 0],
[0, 0, 1000, 0],
[0, 0, 0, 1],
]
rgT = matrix_to_rgtransform(M)
cgT = Transformation.from_matrix(M)
robot = Robot()
viz_planes = []
movel_matcher = re.compile(r"^\s*move([lj]).+((-?\d+\.\d+,?\s?){6}).*$")
for line in script.splitlines():
mo = re.search(movel_matcher, line)
if mo:
if mo.group(1) == "l": # movel
ptX, ptY, ptZ, rX, rY, rZ = mo.group(2).split(",")
pt = Point(float(ptX), float(ptY), float(ptZ))
pt.transform(cgT)
frame = Frame(pt, [1, 0, 0], [0, 1, 0])
R = Rotation.from_axis_angle_vector(
[float(rX), float(rY), float(rZ)], pt)
T = matrix_to_rgtransform(R)
plane = cgframe_to_rgplane(frame)
plane.Transform(T)
viz_planes.append(plane)
else: # movej
joint_values = mo.group(2).split(",")
configuration = Configuration.from_revolute_values(
[float(d) for d in joint_values])
frame = robot.forward_kinematics(configuration)
plane = cgframe_to_rgplane(frame)
plane.Transform(rgT)
viz_planes.append(plane)
return viz_planes
def test_axis_angle_vector():
aav1 = [-0.043, -0.254, 0.617]
R = Rotation.from_axis_angle_vector(aav1)
aav2 = R.axis_angle_vector
assert allclose(aav1, aav2)
"""Example: Create a rotation from and axis and an angle. """ from compas.geometry import Vector from compas.geometry import Rotation aav = Vector(-0.043, -0.254, 0.617) angle, axis = aav.unitized(), aav.length print(angle, axis) R = Rotation.from_axis_angle_vector(aav) axis, angle = R.axis_and_angle print(axis, angle)
"""There are several ways to construct a `Rotation`. """ import math from compas.geometry import Frame from compas.geometry import Rotation R = Rotation.from_axis_and_angle([1, 0, 0], math.radians(30)) R = Rotation.from_axis_and_angle([1, 0, 0], math.radians(30), point=[1, 0, 0]) R = Rotation.from_basis_vectors([0.68, 0.68, 0.27], [-0.67, 0.73, -0.15]) R = Rotation.from_frame(Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])) R = Rotation.from_axis_angle_vector([-0.043, -0.254, 0.617]) R = Rotation.from_quaternion([0.945, -0.021, -0.125, 0.303]) R = Rotation.from_euler_angles([1.4, 0.5, 2.3], static=True, axes='xyz') print(R)
