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)
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)
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
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
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
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()
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'
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)
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)
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))
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)
def convert_frame_wcf_to_tool0_wcf(self, frame_wcf):
return Frame.from_transformation(Transformation.from_frame(frame_wcf) * self.tool_transformation)
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)
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),
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
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()
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
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']
def __init__(self, size):
super(Box, self).__init__()
self.size = _parse_floats(size)
self.geometry = compas.geometry.Box(Frame.worldXY(), *self.size)
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
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)
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
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)