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)