Output:
        :class:`compas_fab.robots.JointTrajectory`
            The calculated trajectory.
"""
from __future__ import print_function
import scriptcontext as sc

from compas.geometry import Frame


guid = str(ghenv.Component.InstanceGuid)
response_key = "response_" + guid
if response_key not in sc.sticky:
    sc.sticky[response_key] = None

frames = [Frame(plane.Origin, plane.XAxis, plane.YAxis) for plane in planes]

if robot and robot.client and start_configuration and compute:
    if robot.client.is_connected:
        options = {
            'max_step': float(max_step),
            'avoid_collisions': bool(avoid_collisions),
            'attached_collision_meshes': list(attached_colllision_meshes),
        }
        sc.sticky[response_key] = robot.plan_cartesian_motion(frames,
                                                              start_configuration=start_configuration,
                                                              group=group,
                                                              options=options)
    else:
        print("Robot client is not connected")
"""Example: 'frame in frame'
"""
from compas.geometry import Point
from compas.geometry import Vector
from compas.geometry import Frame

point = Point(146.00, 150.00, 161.50)
xaxis = Vector(0.9767, 0.0010, -0.214)
yaxis = Vector(0.1002, 0.8818, 0.4609)
F0 = Frame(point, xaxis, yaxis)  # coordinate system F0

point = Point(35., 35., 35.)
xaxis = Vector(0.604, 0.430, 0.671)
yaxis = Vector(-0.631, 0.772, 0.074)
f_lcf = Frame(point, xaxis, yaxis)  # frame f_lcf in F0 (local coordinates)

# frame in global (world) coordinate system
f_wcf = F0.to_world_coordinates(f_lcf)
print(f_wcf)

f_lcf2 = F0.to_local_coordinates(f_wcf)  # world coords back to local coords
print(f_lcf2)  # should equal f_lcf
print(f_lcf == f_lcf2)
예제 #3
0
 def get_tcp_frame_from_tool0_frame(self, frame_tool0):
     """Get the tcp frame from the tool0 frame.
     """
     T = Transformation.from_frame(frame_tool0)
     return Frame.from_transformation(T * self.transformation_tcp_tool0)
예제 #4
0
    def to_vertices_and_faces(self, u=16, triangulated=False):
        """Returns a list of vertices and faces.

        Parameters
        ----------
        u : int, optional
            Number of faces in the "u" direction.
        triangulated: bool, optional
            If True, triangulate the faces.

        Returns
        -------
        list[list[float]]
            A list of vertex locations.
        list[list[int]]
            And a list of faces,
            with each face defined as a list of indices into the list of vertices.

        """
        if u < 3:
            raise ValueError('The value for u should be u > 3.')

        vertices = []
        a = 2 * pi / u
        z = self.height / 2
        for i in range(u):
            x = self.circle.radius * cos(i * a)
            y = self.circle.radius * sin(i * a)
            vertices.append([x, y, z])
            vertices.append([x, y, -z])
        # add v in bottom and top's circle center
        vertices.append([0, 0, z])
        vertices.append([0, 0, -z])

        # transform vertices to cylinder's plane
        frame = Frame.from_plane(self.circle.plane)
        M = matrix_from_frame(frame)
        vertices = transform_points(vertices, M)

        faces = []
        # side faces
        for i in range(0, u * 2, 2):
            faces.append([i, i + 1, (i + 3) % (u * 2), (i + 2) % (u * 2)])
        # top and bottom circle faces
        for i in range(0, u * 2, 2):
            top = [i, (i + 2) % (u * 2), len(vertices) - 2]
            bottom = [i + 1, (i + 3) % (u * 2), len(vertices) - 1]
            faces.append(top)
            faces.append(bottom[::-1])

        if triangulated:
            triangles = []
            for face in faces:
                if len(face) == 4:
                    triangles.append(face[0:3])
                    triangles.append([face[0], face[2], face[3]])
                else:
                    triangles.append(face)
            faces = triangles

        return vertices, faces
예제 #5
0
def unroll(zone):
    unrolled = []
    for quadmesh, trimesh in zone:
        flatmesh = trimesh.copy()
        fkeys = list(trimesh.faces_where({'count': 0}))

        for fkey in fkeys:
            nbrs = trimesh.face_neighbors(fkey)
            if len(nbrs) == 1:
                root = fkey
                break
            root = None

        for key in trimesh.face_vertices(root):
            if trimesh.vertex_degree(key) == 2:
                corner = key
                break
            corner = None

        u = corner
        v = trimesh.face_vertex_descendant(root, u)

        origin = trimesh.vertex_coordinates(u)
        zaxis = trimesh.face_normal(root, unitized=True)
        xaxis = normalize_vector(trimesh.edge_direction(u, v))
        yaxis = normalize_vector(cross_vectors(zaxis, xaxis))
        frame = Frame(origin, xaxis, yaxis)

        frame_to = Frame.worldXY()

        T = Transformation.from_frame_to_frame(frame, frame_to)

        for key in trimesh.face_vertices(root):
            x, y, z = trimesh.vertex_coordinates(key)
            point = Point(x, y, z)
            point.transform(T)
            flatmesh.set_vertex_attributes(key, 'xyz', point)

        tovisit = deque([root])
        visited = set([root])
        while tovisit:
            fkey = tovisit.popleft()
            for u, v in trimesh.face_halfedges(fkey):
                nbr = trimesh.halfedge[v][u]
                if nbr is None:
                    continue
                if nbr in visited:
                    continue
                tovisit.append(nbr)
                visited.add(nbr)

                origin = trimesh.vertex_coordinates(v)
                zaxis = trimesh.face_normal(nbr, unitized=True)
                xaxis = normalize_vector(trimesh.edge_direction(v, u))
                yaxis = normalize_vector(cross_vectors(xaxis, zaxis))
                frame = Frame(origin, xaxis, yaxis)

                origin = flatmesh.vertex_coordinates(v)
                zaxis = [0, 0, 1.0]
                xaxis = normalize_vector(flatmesh.edge_direction(v, u))
                yaxis = normalize_vector(cross_vectors(xaxis, zaxis))
                frame_to = Frame(origin, xaxis, yaxis)

                T = Transformation.from_frame_to_frame(frame, frame_to)
                w = trimesh.face_vertex_ancestor(nbr, v)
                x, y, z = trimesh.vertex_coordinates(w)
                point = Point(x, y, z)
                point.transform(T)
                flatmesh.set_vertex_attributes(w, 'xyz', point)

        for key, attr in quadmesh.vertices(True):
            x, y, z = flatmesh.vertex_coordinates(key)
            attr['x'] = x
            attr['y'] = y
            attr['z'] = z

        unrolled.append(quadmesh)

    return unrolled
예제 #6
0
        def inverse_kinematics(robot, frame_WCF, start_configuration=None, group=None, options=None):
            # TODO use input here instead of class attributes
                            #    avoid_collisions=True, constraints=None,
                            #    attempts=8, attached_collision_meshes=None,
                            #    return_full_configuration=False,
                            #    cull=False,
                            #    return_closest_to_start=False,
                            #    return_idxs=None):
            avoid_collisions = is_valid_option(options, 'avoid_collisions', True)
            constraints = is_valid_option(options, 'constraints', None)
            attempts = is_valid_option(options, 'attempts', 8)
            attached_collision_meshes = is_valid_option(options, 'attached_collision_meshes', None)
            return_full_configuration = is_valid_option(options, 'return_full_configuration', False)
            cull = is_valid_option(options, 'cull', False)
            return_closest_to_start = is_valid_option(options, 'return_closest_to_start', False)
            return_idxs = is_valid_option(options, 'return_idxs', None)

            if start_configuration:
                base_frame = robot.get_base_frame(self.group, full_configuration=start_configuration)
                self.update_base_transformation(base_frame)
                # in_collision = robot.client.configuration_in_collision(start_configuration)
                # if in_collision:
                #    raise ValueError("Start configuration already in collision")

            # frame_tool0_RCF = self.convert_frame_wcf_to_frame_tool0_rcf(frame_WCF)
            frame_tool0_RCF = Frame.from_transformation(self.base_transformation * Transformation.from_frame(frame_WCF) * self.tool_transformation)

            # call the ik function
            configurations = self.function(frame_tool0_RCF)

            # The ik solution for 6 axes industrial robots returns by default 8
            # configurations, which are sorted. That means, the if you call ik
            # on 2 frames that are close to each other, and compare the 8
            # configurations of the first one with the 8 of the second one at
            # their respective indices, then these configurations are 'close' to
            # each other. That is why for certain use cases, e.g. custom cartesian
            # path planning it makes sense to keep the sorting and set the ones
            # that are out of joint limits or in collison to `None`.

            if return_idxs:
                configurations = [configurations[i] for i in return_idxs]

            # add joint names to configurations
            self.add_joint_names_to_configurations(configurations)

            # fit configurations within joint bounds (sets those to `None` that are not working)
            self.try_to_fit_configurations_between_bounds(configurations)
            # check collisions for all configurations (sets those to `None` that are not working)
            # TODO defer collision checking
            # if robot.client:
            #     robot.client.check_configurations_for_collision(configurations)

            if return_closest_to_start:
                diffs = [c.max_difference(start_configuration) for c in configurations if c is not None]
                if len(diffs):
                    idx = diffs.index(min(diffs))
                    return configurations[idx]  # only one
                return None

            if cull:
                configurations = [c for c in configurations if c is not None]

            return configurations
예제 #7
0
파일: plane.py 프로젝트: itaycsguy/compas
    base = Point(0.0, 0.0, 0.0)
    normal = Vector(1.0, 0.0, 0.0)

    plane = Plane(base, normal)

    print(plane)
    print(plane.d)

    a, b, c = normal

    p = [1.0, 0.0, 1.0]

    d = a * p[0] + b * p[1] + c * p[2]

    print(d)
    print(d <= plane.d)

    f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
    T = Transformation.from_frame(f)
    plane = Plane.worldXY()
    plane.transform(T)
    print(plane)

    data = {'point': [0.0, 0.0, 0.0], 'normal': [0.0, 0.0, 1.0]}
    plane = Plane.from_data(data)
    print(plane)

    import doctest
    doctest.testmod()
예제 #8
0
 def __init__(self, mesh, id, frame=None, root_name=None):
     self.id = id
     self.mesh = mesh
     self.frame = frame or Frame.worldXY()
     self.root_name = root_name or 'world'
예제 #9
0
        h = np.minimum(np.maximum(0.5 - 0.5 * (da + db)/self.r, 0), 1)
        return (da * (1 - h) + h * -db) + self.r * h * (1 - h)


# ==============================================================================
# Main
# ==============================================================================

if __name__ == "__main__":
    from compas_vol.primitives import VolSphere, VolBox
    from compas.geometry import Box, Frame, Point, Sphere
    import numpy as np
    import matplotlib.pyplot as plt

    s = Sphere(Point(5, 6, 0), 9)
    b = Box(Frame.worldXY(), 20, 15, 10)
    vs = VolSphere(s)
    vb = VolBox(b, 2.5)
    u = SmoothSubtraction(vb, vs, 2.5)
    # for y in range(-15, 15):
    #     s = ''
    #     for x in range(-30, 30):
    #         d = u.get_distance(Point(x * 0.5, y, 0))
    #         if d < 0:
    #             s += 'x'
    #         else:
    #             s += '.'
    #     print(s)

    x, y, z = np.ogrid[-15:15:50j, -15:15:50j, -15:15:50j]
    d = u.get_distance_numpy(x, y, z)
예제 #10
0
파일: capsule.py 프로젝트: yishizu/compas
    def to_vertices_and_faces(self, **kwargs):
        """Returns a list of vertices and faces"""

        u = kwargs.get('u') or 10
        v = kwargs.get('v') or 10
        if u < 3:
            raise ValueError('The value for u should be u > 3.')
        if v < 3:
            raise ValueError('The value for v should be v > 3.')
        if v % 2 == 1:
            v += 1

        theta = pi / v
        phi = pi * 2 / u
        hpi = pi * 0.5
        halfheight = self.line.length / 2
        sidemult = -1
        capswitch = 0

        vertices = []
        for i in range(1, v + 1):
            for j in range(u):
                a = i + capswitch
                tx = self.radius * cos(a * theta - hpi) * cos(j * phi)
                ty = self.radius * cos(a * theta - hpi) * sin(j * phi)
                tz = self.radius * sin(a * theta - hpi) + sidemult * halfheight
                vertices.append([tx, ty, tz])
            # switch from lower pole cap to upper pole cap
            if i == v / 2 and sidemult == -1:
                capswitch = -1
                sidemult *= -1

        vertices.append([0, 0, halfheight + self.radius])
        vertices.append([0, 0, -halfheight - self.radius])

        # move points to correct location in space
        plane = Plane(self.line.midpoint, self.line.direction)
        frame = Frame.from_plane(plane)
        M = matrix_from_frame(frame)
        vertices = transform_points(vertices, M)

        faces = []

        # south pole triangle fan
        sp = len(vertices) - 1
        for j in range(u):
            faces.append([sp, (j + 1) % u, j])

        for i in range(v - 1):
            for j in range(u):
                jj = (j + 1) % u
                a = i * u + j
                b = i * u + jj
                c = (i + 1) * u + jj
                d = (i + 1) * u + j
                faces.append([a, b, c, d])

        # north pole triangle fan
        np = len(vertices) - 2
        for j in range(u):
            nc = len(vertices) - 3 - j
            nn = len(vertices) - 3 - (j + 1) % u
            faces.append([np, nn, nc])

        return vertices, faces
from ex50_abb_linear_axis_robot import robot

# SETTINGS =====================================================================

HERE = os.path.dirname(__file__)
DATA = os.path.join(HERE, '../data')
PATH_FROM = os.path.join(DATA, '52_wall_buildable.json')
PATH_TO = os.path.join(DATA, '53_wall_paths.json')

robot.client = RosClient()
robot.client.run()

group = "abb"
#group = "axis_abb" # Try with this as well...

picking_frame = Frame([1.926, 1.5, 1], [0, 1, 0], [1, 0, 0])
picking_configuration = Configuration.from_prismatic_and_revolute_values([-1.800], [0.569, 0.849, -0.235, 6.283, 0.957, 2.140])

save_vector = Vector(0, 0, 0.1)
saveframe_pick = Frame(picking_frame.point + save_vector, picking_frame.xaxis, picking_frame.yaxis)

# Load assembly
assembly = Assembly.from_json(PATH_FROM)

# COLLISION SETTINGS ===========================================================

# Add platform as collision mesh
package = "abb_linear_axis"
mesh = Mesh.from_stl(os.path.join(DATA, 'robot_description', package, 'meshes', 'collision', 'platform.stl'))
robot.add_collision_mesh_to_planning_scene('platform', mesh)
예제 #12
0
 def __init__(self, distobj=None, frame=Frame.worldXY()):
     self.distobj = distobj
     self.frame = frame
     transform = matrix_from_frame(self.frame)
     self.inversetransform = matrix_inverse(transform)
element_height = 0.014
safelevel_height = 0.05

with RosClient('localhost') as client:
    robot = client.load_robot()

    # 1. Set tool
    robot.attach_tool(tool)

    # 2. Define start configuration
    start_configuration = Configuration.from_revolute_values(
        [-5.961, 4.407, -2.265, 5.712, 1.571, -2.820])

    # 3. Define frames
    picking_frame = Frame([-0.43, 0, element_height], [1, 0, 0], [0, 1, 0])
    safelevel_picking_frame = Frame(
        [-0.43, 0, element_height + safelevel_height], [1, 0, 0], [0, 1, 0])
    frames = [picking_frame, safelevel_picking_frame]

    # 4. Convert frames to tool0_frames
    frames_tool0 = robot.from_tcf_to_t0cf(frames)

    trajectory = robot.plan_cartesian_motion(frames_tool0,
                                             start_configuration,
                                             options=dict(max_step=0.01))

    print("Computed cartesian path with %d configurations, " %
          len(trajectory.points))
    print("following %d%% of requested trajectory." %
          (trajectory.fraction * 100))
예제 #14
0
    data = json.load(f)

# create tool from json
filepath = os.path.join(DATA, "vacuum_gripper.json")
tool = Tool.from_json(filepath)

# define brick dimensions
width, length, height = data['brick_dimensions']

# little tolerance to not 'crash' into collision objects
tolerance_vector = Vector.from_data(data['tolerance_vector'])

savelevel_vector = Vector.from_data(data['savelevel_vector'])

# define target frame
target_frame = Frame([-0.26, -0.28, height], [1, 0, 0], [0, 1, 0])
target_frame.point += tolerance_vector

# create Element and move it to target frame
element = Element.from_data(data['brick'])

# create Assembly with element at target_frame
assembly = Assembly()
T = Transformation.from_frame_to_frame(element.frame, target_frame)
assembly.add_element(element.transformed(T))

# Bring the element's mesh into the robot's tool0 frame
element_tool0 = element.copy()
T = Transformation.from_frame_to_frame(element_tool0.gripping_frame,
                                       tool.frame)
element_tool0.transform(T)
from compas.geometry import Frame
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration
from helpers import show_trajectory

with RosClient('localhost') as client:
    robot = client.load_robot()
    group = robot.main_group_name

    frames = []
    frames.append(Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]))
    frames.append(Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0]))

    start_configuration = Configuration.from_revolute_values([-0.042, 0.033, -2.174, 5.282, -1.528, 0.000])

    trajectory = robot.plan_cartesian_motion(frames,
                                             start_configuration,
                                             group=group,
                                             options=dict(
                                                 max_step=0.01,
                                                 avoid_collisions=True,
                                             ))

    print("Computed cartesian path with %d configurations, " % len(trajectory.points))
    print("following %d%% of requested trajectory." % (trajectory.fraction * 100))
    print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)

    show_trajectory(trajectory)
 def box_update(self):
     self.board_frame = Frame(self.centre_point, self.length_vector,
                              self.width_vector)
     self.box = Box(self.board_frame, self.length, self.width,
                    self.height)
예제 #17
0
 def convert_frame_wcf_to_tool0_wcf(self, frame_wcf):
     return Frame.from_transformation(Transformation.from_frame(frame_wcf) * self.tool_transformation)
예제 #18
0
from compas.geometry import Frame
from compas_fab.backends import RosClient
from compas_fab.robots.ur5 import Robot

with RosClient() as client:
    robot = Robot(client)

    frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
    start_configuration = robot.init_configuration()

    configuration = robot.inverse_kinematics(frame_WCF, start_configuration)

    print("Found configuration", configuration)
예제 #19
0
    desirial_configs = [
        Configuration.from_data(data) for data in desirial_data
    ]

    return desirial_configs


# RUN code
command_line = False  # toggle off to run in GH

if command_line:
    # Use the following to test from the command line
    # Or copy solution_viewer.ghx next to the folder where you created assignment_03.py to visualize the same in Grasshopper
    if __name__ == '__main__':
        frame_list = [
            Frame(Point(0.084, 0.319, 0.175), Vector(0.000, 0.000, -1.000),
                  Vector(0.000, 1.000, 0.000)),
            Frame(Point(0.152, 0.317, 0.175), Vector(0.000, 0.000, -1.000),
                  Vector(0.000, 1.000, 0.000)),
            Frame(Point(0.220, 0.315, 0.175), Vector(0.000, 0.000, -1.000),
                  Vector(0.000, 1.000, 0.000)),
            Frame(Point(0.288, 0.313, 0.175), Vector(0.000, 0.000, -1.000),
                  Vector(0.000, 1.000, 0.000)),
            Frame(Point(0.357, 0.310, 0.175), Vector(0.000, 0.000, -1.000),
                  Vector(0.000, 1.000, 0.000)),
            Frame(Point(0.425, 0.308, 0.175), Vector(0.000, 0.000, -1.000),
                  Vector(0.000, 1.000, 0.000)),
            Frame(Point(0.493, 0.306, 0.175), Vector(0.000, 0.000, -1.000),
                  Vector(0.000, 1.000, 0.000)),
            Frame(Point(0.561, 0.303, 0.175), Vector(0.000, 0.000, -1.000),
                  Vector(0.000, 1.000, 0.000)),
            Frame(Point(0.629, 0.301, 0.175), Vector(0.000, 0.000, -1.000),
예제 #20
0
 def _set_data(self, data):
     super(ToolModel, self)._set_data(data)
     self.frame = Frame.from_data(data['frame'])
     self.name = self.name or 'attached_tool'
     self.link_name = data['link_name'] if 'link_name' in data else None
예제 #21
0
    from .kinematics.path_calculation import smallest_joint_pose

    ur = UR10()

    q = [4.6733, -3.39529, 1.5404, -2.90962, -1.58137, 1.59137]
    pose = [-206.258, -865.946, 606.26, 0.037001, -0.044931, 1.55344]

    pose = [
        -576.673, -717.359, 419.691, -1.41669, -0.88598900000000003,
        0.96527600000000002
    ]
    q = [
        4.32717, -3.57284, 1.62216, -2.58119, -0.00038495899999999998, 1.45664
    ]
    print("q: {0}".format(smallest_joint_pose(q)))
    f = Frame.from_pose_axis_angle_vector(pose)
    print("f: {0}".format(f))

    R0, R1, R2, R3, R4, R5 = ur.get_forward_transformations(q)
    f = ur.get_transformed_tool_frames(R5)[0]
    print("f ?: {0}".format(f))

    #f = ur.forward_kinematics(q)
    #print("f 1: {0}".format(f))
    q = ur.inverse_kinematics(f)

    print()
    for x in q:
        print(smallest_joint_pose(x))
    print()
예제 #22
0
    def to_vertices_and_faces(self, u=16, v=16, triangulated=False):
        """Returns a list of vertices and faces.

        Parameters
        ----------
        u : int, optional
            Number of faces in the 'u' direction.
        v : int, optional
            Number of faces in the 'v' direction.
        triangulated: bool, optional
            If True, triangulate the faces.

        Returns
        -------
        list[list[float]]
            A list of vertex locations.
        list[list[int]]
            And a list of faces,
            with each face defined as a list of indices into the list of vertices.

        """
        if u < 3:
            raise ValueError('The value for u should be u > 3.')
        if v < 3:
            raise ValueError('The value for v should be v > 3.')
        if v % 2 == 1:
            v += 1

        theta = pi / v
        phi = pi * 2 / u
        hpi = pi * 0.5
        halfheight = self.line.length / 2
        sidemult = -1
        capswitch = 0

        vertices = []
        for i in range(1, v + 1):
            for j in range(u):
                a = i + capswitch
                tx = self.radius * cos(a * theta - hpi) * cos(j * phi)
                ty = self.radius * cos(a * theta - hpi) * sin(j * phi)
                tz = self.radius * sin(a * theta - hpi) + sidemult * halfheight
                vertices.append([tx, ty, tz])
            # switch from lower pole cap to upper pole cap
            if i == v / 2 and sidemult == -1:
                capswitch = -1
                sidemult *= -1

        vertices.append([0, 0, halfheight + self.radius])
        vertices.append([0, 0, -halfheight - self.radius])

        # move points to correct location in space
        plane = Plane(self.line.midpoint, self.line.direction)
        frame = Frame.from_plane(plane)
        M = matrix_from_frame(frame)
        vertices = transform_points(vertices, M)

        faces = []

        # south pole triangle fan
        sp = len(vertices) - 1
        for j in range(u):
            faces.append([sp, (j + 1) % u, j])

        for i in range(v - 1):
            for j in range(u):
                jj = (j + 1) % u
                a = i * u + j
                b = i * u + jj
                c = (i + 1) * u + jj
                d = (i + 1) * u + j
                faces.append([a, b, c, d])

        # north pole triangle fan
        np = len(vertices) - 2
        for j in range(u):
            nc = len(vertices) - 3 - j
            nn = len(vertices) - 3 - (j + 1) % u
            faces.append([np, nn, nc])

        if triangulated:
            triangles = []
            for face in faces:
                if len(face) == 4:
                    triangles.append(face[0:3])
                    triangles.append([face[0], face[2], face[3]])
                else:
                    triangles.append(face)
            faces = triangles

        return vertices, faces
예제 #23
0
import logging

from compas.datastructures import Mesh
from compas.geometry import Frame

import compas_fab
from compas_fab.robots import rfl
from compas_fab.backends import VrepClient

# Configure logging to DEBUG to see detailed timing of the path planning
logging.basicConfig(level=logging.DEBUG)

# Configure parameters for path planning
start_pose = Frame((7.453, 2.905, 0.679), (1, 0, 0), (0, -1, 0))
goal_pose = Frame((5.510, 5.900, 1.810), (0, 0, -1), (0, 1, 0))
planner_id = 'rrtconnect'
max_trials = 1
resolution = 0.02
building_member = Mesh.from_obj(
    compas_fab.get('planning_scene/timber_beam.obj'))
structure = [
    Mesh.from_obj(compas_fab.get('planning_scene/timber_structure.obj'))
]
metric = [0.1] * 9
fast_search = True

with VrepClient(debug=True) as client:
    robot = rfl.Robot('A', client=client)
    client.planner.pick_building_member(robot, building_member, start_pose)
    group = robot.model.attr['index']
예제 #24
0
 def __init__(self, size):
     super(Box, self).__init__()
     self.size = _parse_floats(size)
     self.geometry = compas.geometry.Box(Frame.worldXY(), *self.size)
예제 #25
0
def vrep_pose_to_frame(pose, scale):
    return Frame.from_list(floats_from_vrep(pose, scale))
 def box_update(elmnt):
     elmnt.board_frame = Frame(self.centre_point, self.length_vector,
                               self.width_vector)
     elmnt.box = Box(self.board_frame, self.length, self.width, self.height)
     return elmnt.board_frame, elmnt.box
예제 #27
0
 def get_tool0_frame_from_tcp_frame(self, frame_tcp):
     """Get the tool0 frame (frame at robot) from the tool frame (frame_tcp).
     """
     T = Transformation.from_frame(frame_tcp)
     return Frame.from_transformation(T * self.transformation_tool0_tcp)
    def board_geometry_setup():
        for my_element in self.elements():
            my_board = my_element[1]
            if my_board.layer % 2 == 0:
                my_frame = self.origin_fr
                layer_standard_length = self.primary_length
            else:
                my_frame = self.sec_fr
                layer_standard_length = self.secondary_length

            my_dir1 = normalize_vector(my_frame[1])
            my_dir2 = normalize_vector(my_frame[2])

            dist = my_board.grid_position

            if my_board.location == "high":
                if not my_board.supporter:
                    length_attribute_1 = layer_standard_length - my_board.length / 2
                else:
                    length_attribute_1 = layer_standard_length - my_board.width / 2
            elif my_board.location == "low":
                if not my_board.supporter:
                    length_attribute_1 = my_board.length / 2
                else:
                    length_attribute_1 = my_board.width / 2
            else:
                length_attribute_1 = layer_standard_length / 2

            # position parallel to the boards (if not sup)
            my_vec1 = scale_vector(my_dir1, length_attribute_1)
            # position perpendicular to board direction (if not sup)
            my_vec2 = scale_vector(my_dir2, dist)
            # height vector
            my_vec3 = Vector(0, 0, my_board.z_drop - my_board.height / 2)
            my_centre = self.origin_pt + my_vec1 + my_vec2 + my_vec3
            my_board.centre_point = my_centre

            my_board.drop_point = my_centre + Vector(0, 0, my_board.height / 2)
            if not my_board.supporter:
                my_board.length_vector = normalize_vector(my_vec1)
                my_board.width_vector = normalize_vector(my_vec2)
            else:
                my_board.length_vector = normalize_vector(my_vec2)
                my_board.width_vector = normalize_vector(my_vec1)

            old_centre = my_board.center
            T = Translation(my_centre - old_centre)

            self.network.node[my_board.global_count]['x'] = my_centre[0]
            self.network.node[my_board.global_count]['y'] = my_centre[1]
            self.network.node[my_board.global_count]['z'] = my_centre[2]

            my_board.transform(T)
            my_board.board_frame = Frame(my_board.centre_point,
                                         my_board.length_vector,
                                         my_board.width_vector)
            my_board.tool_frame = Frame(my_board.drop_point,
                                        my_board.length_vector,
                                        my_board.width_vector)
            my_board.box = Box(my_board.board_frame, my_board.length,
                               my_board.width, my_board.height)
예제 #29
0
 def get_transformed_tool_frames(self, T5):
     T = self.get_tool0_transformation(T5)
     tool0_frame = Frame.from_transformation(T)
     tcp_frame = Frame.from_transformation(T *
                                           self.transformation_tcp_tool0)
     return tool0_frame, tcp_frame
예제 #30
0
import math
from compas.geometry import Frame
from compas_fab.backends import RosClient
from compas_fab.robots import Configuration

with RosClient('localhost') as client:
    robot = client.load_robot()
    group = robot.main_group_name

    frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
    tolerance_position = 0.001
    tolerance_axes = [math.radians(1)] * 3

    start_configuration = Configuration.from_revolute_values(
        [-0.042, 4.295, 0, -3.327, 4.755, 0.])

    # create goal constraints from frame
    goal_constraints = robot.constraints_from_frame(frame, tolerance_position,
                                                    tolerance_axes, group)

    trajectory = robot.plan_motion(goal_constraints,
                                   start_configuration,
                                   group,
                                   planner_id='RRT')

    print("Computed kinematic path with %d configurations." %
          len(trajectory.points))
    print(
        "Executing this path at full speed would take approx. %.3f seconds." %
        trajectory.time_from_start)