def __init__(self, robot, group, function, base_frame=None, tool_frame=None):
# TODO create with class `Tool`, not tool_frame!!
self.group = group
self.joints = robot.get_configurable_joints(group=group)
self.joint_names = robot.get_configurable_joint_names(group=group)
self.function = function
self.base_transformation = Transformation.from_frame(base_frame).inverse() if base_frame else None
self.tool_transformation = Transformation.from_frame(tool_frame).inverse() if tool_frame else None
def transform(self, T):
"""Transform the frame.
Parameters
----------
T : :class:`compas.geometry.Transformation`
The transformation.
Examples
--------
>>> from compas.geometry import Transformation
>>> f1 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
>>> T = Transformation.from_frame(f1)
>>> f2 = Frame.worldXY()
>>> f2.transform(T)
>>> f1 == f2
True
"""
# replace this by function call
X = T * Transformation.from_frame(self)
point = X.translation_vector
xaxis, yaxis = X.basis_vectors
self.point = point
self.xaxis = xaxis
self.yaxis = yaxis
def _create(self, link, parent_transformation):
"""Private function called during initialisation to transform origins and axes.
Parameters
----------
link : :class:`compas.robots.Link`
Link instance to create.
parent_transformation : :class:`Transformation`
Parent transformation to apply to the link when creating the structure.
"""
if link is None: # some urdfs would fail here otherwise
return
for item in itertools.chain(link.visual, link.collision):
if item.origin:
# transform visual or collision geometry with the transformation specified in origin
transformation = Transformation.from_frame(item.origin)
item.init_transformation = parent_transformation * transformation
else:
item.init_transformation = parent_transformation
for child_joint in link.joints:
child_joint._create(parent_transformation)
# Recursively call creation
self._create(child_joint.child_link,
child_joint.current_transformation)
def convert_frame_wcf_to_frame_tool0_rcf(self, frame_wcf, base_transformation=None, tool_transformation=None):
T = Transformation.from_frame(frame_wcf)
if base_transformation:
T = base_transformation * T
if tool_transformation:
T = T * tool_transformation
return Frame.from_transformation(T)
def read_mesh_from_filename(self, filename, meshcls):
if not os.path.isfile(filename):
raise FileNotFoundError("No such file: '%s'" % filename)
extension = filename[(filename.rfind(".") + 1):]
if extension == "dae": # no dae support yet
#mesh = Mesh.from_dae(filename)
obj_filename = filename.replace(".dae", ".obj")
if os.path.isfile(obj_filename):
mesh = Mesh.from_obj(obj_filename)
# former DAE files have yaxis and zaxis swapped
# TODO: already fix in conversion to obj
frame = Frame([0,0,0], [1,0,0], [0,0,1])
T = Transformation.from_frame(frame)
mesh_transform(mesh, T)
else:
raise FileNotFoundError("Please convert '%s' into an OBJ file, \
since DAE is currently not supported \
yet." % filename)
elif extension == "obj":
mesh = Mesh.from_obj(filename)
elif extension == "stl":
mesh = Mesh.from_stl(filename)
else:
raise ValueError("%s file types not yet supported" %
extension.upper())
return meshcls(mesh)
def from_t0cf_to_tcf(self, frames_t0cf):
"""Converts frames at the robot's flange (tool0 frame) to frames at the robot's tool tip (tcf frame).
Parameters
----------
frames_t0cf : list[:class:`compas.geometry.Frame`]
Frames (in WCF) at the robot's flange (tool0).
Returns
-------
list[:class:`compas.geometry.Frame`]
Frames (in WCF) at the robot's tool tip (tcf).
Examples
--------
>>> import compas
>>> from compas.datastructures import Mesh
>>> from compas.geometry import Frame
>>> mesh = Mesh.from_stl(compas.get('cone.stl'))
>>> frame = Frame([0.14, 0, 0], [0, 1, 0], [0, 0, 1])
>>> tool = ToolModel(mesh, frame)
>>> frames_t0cf = [Frame((-0.363, 0.003, -0.147), (0.388, -0.351, -0.852), (0.276, 0.926, -0.256))]
>>> tool.from_t0cf_to_tcf(frames_t0cf)
[Frame(Point(-0.309, -0.046, -0.266), Vector(0.276, 0.926, -0.256), Vector(0.879, -0.136, 0.456))]
"""
Te = Transformation.from_frame_to_frame(Frame.worldXY(), self.frame)
return [
Frame.from_transformation(Transformation.from_frame(f) * Te)
for f in frames_t0cf
]
def draw_box(self, box, color=None):
geo = p3js.BoxBufferGeometry(width=box.xsize,
height=box.zsize,
depth=box.ysize,
widthSegments=box.xsize,
heightSegments=box.zsize,
depthSegments=box.ysize)
mat = material_from_color(color)
mesh = p3js.Mesh(geometry=geo, material=mat)
Tinit = Translation([box.xsize/2, box.ysize/2, box.zsize/2])
Sc, Sh, R, T, P = (Transformation.from_frame(box.frame) * Tinit).decompose()
mesh.quaternion = R.quaternion.xyzw
mesh.position = list(T.translation)
self.geometry.append(mesh)
return mesh
def _get_current_base_frame(self, full_configuration, group):
"""Returns the group's current base frame, if the robot is in full_configuration.
The base_frame of a planning group can change if a parent joint was
transformed. This function performs a forward kinematic request with the
full configuration to retrieve the (possibly) transformed base_frame of
planning group. This function is only used in plan_motion since other
services, such as ik or plan_cartesian_motion, do not use the
transformed base_frame as the group's local coordinate system.
Parameters
----------
full_configuration : :class:`compas_fab.robots.Configuration`
The (full) configuration from which the group's base frame is
calculated.
group : str
The planning group for which we want to get the transformed base frame.
Returns
-------
:class:`compas.geometry.Frame`
Examples
--------
"""
self.ensure_client()
base_link = self.get_base_link_name(group)
# the group's original base_frame
base_frame = self.get_base_frame(group)
joint_names = self.get_configurable_joint_names()
joint_positions = self._get_scaled_joint_positions_from_start_configuration(
full_configuration)
# ideally we would call this with the planning group that includes all
# configurable joints, but we cannot be sure that this group exists.
# That's why we have to do the workaround with the Transformation.
response = self.client.forward_kinematics(joint_positions, base_link,
group, joint_names,
base_link)
base_frame_RCF = response.pose_stamped[0].pose.frame
base_frame_RCF.point *= self.scale_factor
T = Transformation.from_frame(base_frame)
return base_frame_RCF.transformed(T)
def attach_mesh(self, mesh, name, link=None, frame=None):
"""Rigidly attaches a compas mesh to a given link for visualization.
Parameters
----------
mesh : :class:`compas.datastructures.Mesh`
The mesh to attach to the robot model.
name : str
The identifier of the mesh.
link : :class:`compas.robots.Link`
The link within the robot model or tool model to attach the mesh to. Optional.
Defaults to the model's end effector link.
frame : :class:`compas.geometry.Frame`
The frame of the mesh. Defaults to :meth:`compas.geometry.Frame.worldXY`.
Returns
-------
None
"""
if not link:
link = self.model.get_end_effector_link()
transformation = Transformation.from_frame(
frame) if frame else Transformation()
sample_geometry = None
while sample_geometry is None:
sample_geometry = link.collision[
0] if link.collision else link.visual[
0] if link.visual else None
link = self.model.get_link_by_name(link.parent_joint.parent.link)
native_mesh = self.create_geometry(mesh)
init_transformation = transformation * sample_geometry.init_transformation
self.transform(
native_mesh,
sample_geometry.current_transformation * init_transformation)
item = LinkItem()
item.native_geometry = [native_mesh]
item.init_transformation = init_transformation
item.current_transformation = sample_geometry.current_transformation
self.attached_items.setdefault(link.name, {})[name] = item
def represent_frame_in_transformed_RCF(self,
frame_WCF,
configuration,
group=None):
"""Returns the frame's representation the current robot's coordinate frame (RCF).
Parameters
----------
frame_WCF (:class:`Frame`): A frame in the world coordinate frame.
configuration (:class:`Configuration`): The (full) configuration
from which the group's base frame is calculated.
group (str, optional): The planning group.
Defaults to the robot's main planning group.
Examples
--------
"""
self.ensure_client()
if not group:
group = self.main_group_name # ensure semantics
base_link = self.get_base_link_name(group)
joint_names = self.get_configurable_joint_names()
if len(joint_names) != len(configuration.values):
raise ValueError("Please pass a configuration with %d values" %
len(joint_names))
joint_positions = configuration.values
joint_positions = self.scale_joint_values(joint_positions,
1. / self.scale_factor)
response = self.client.forward_kinematics(joint_positions, base_link,
group, joint_names,
base_link)
base_frame_RCF = response.pose_stamped[
0].pose.frame # the group's transformed base_frame in RCF
base_frame = self.get_base_frame(
group) # the group's original base_frame
T = Transformation.from_frame(base_frame)
base_frame = base_frame_RCF.transformed(T) # the current base frame
T = Transformation.from_frame_to_frame(base_frame, Frame.worldXY())
return frame_WCF.transformed(T)
def get_TCP_pose_from_joint_values(joint_values,
robot_model='ur3',
tcp_tf_list=[1e-3 * 80.525, 0, 0]):
if robot_model == 'ur3':
fk_fn = ikfast_ur3.get_fk
elif robot_model == 'ur5':
fk_fn = ikfast_ur5.get_fk
else:
raise ValueError('Not supported robot model!')
pb_pose = compute_forward_kinematics(fk_fn, joint_values)
mount_frame = Frame_from_pb_pose(pb_pose)
if tcp_tf_list:
world_from_mount = Transformation.from_frame(mount_frame)
mount_from_TCP = Translation(tcp_tf_list)
TCP_frame = Frame.from_transformation(\
Transformation.concatenate(world_from_mount, mount_from_TCP))
return TCP_frame.to_data()
else:
return mount_frame.to_data()
def _mesh_import(url, filename):
"""Internal function to load meshes using the correct loader.
Name and file might be the same but not always, e.g. temp files."""
file_extension = _get_file_format(url)
if file_extension not in SUPPORTED_FORMATS:
raise NotImplementedError(
'Mesh type not supported: {}'.format(file_extension))
print(filename)
if file_extension == "dae": # no dae support yet
#mesh = Mesh.from_dae(filename)
obj_filename = filename.replace(".dae", ".obj")
if os.path.isfile(obj_filename):
mesh = Mesh.from_obj(obj_filename)
# former DAE files have yaxis and zaxis swapped
# TODO: already fix in conversion to obj
frame = Frame([0,0,0], [1,0,0], [0,0,1])
T = Transformation.from_frame(frame)
mesh_transform(mesh, T)
return mesh
else:
raise FileNotFoundError("Please convert '%s' into an OBJ file, \
since DAE is currently not supported \
yet." % filename)
if file_extension == 'obj':
return Mesh.from_obj(filename)
elif file_extension == 'stl':
return Mesh.from_stl(filename)
elif file_extension == 'ply':
return Mesh.from_ply(filename)
raise Exception
try:
response = robot.compute_cartesian_path(frames_WCF=frames,
start_configuration=last_configuration,
max_step=0.01,
avoid_collisions=True,
group=group,
path_constraints=None,
attached_collision_object=aco)
if response.fraction == 1.:
paths.append(response.solution)
last_configuration = response.configurations[-1]
else:
print("Cartesian computed only %d percent of the path" % (response.fraction * 100))
break
except RosError as error:
print("Cartesian:", error)
break
# Update attributes
assembly.set_vertex_attribute(key, 'is_planned', True)
assembly.set_vertex_attribute(key, 'paths', [path.msg for path in paths])
# Add placed brick to planning scene
brick_transformed = mesh_transformed(brick, Transformation.from_frame(placing_frame))
robot.append_collision_mesh_to_planning_scene('brick_wall', brick)
assembly.to_json(PATH_TO)
robot.client.close()
robot.client.terminate()
def current_transformation(self):
"""Current transformation of the joint."""
return Transformation.from_frame(self.current_origin)
def test_from_frame():
f1 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
T = Transformation.from_frame(f1)
f2 = Frame.from_transformation(T)
assert np.allclose(f1, f2)
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
def convert_frame_wcf_to_tool0_wcf(self, frame_wcf):
return Frame.from_transformation(Transformation.from_frame(frame_wcf) * self.tool_transformation)
def test_collision_checker(abb_irb4600_40_255_setup, itj_TC_g1_cms,
itj_beam_cm, column_obstacle_cm, base_plate_cm,
itj_tool_changer_grasp_transf,
itj_gripper_grasp_transf, itj_beam_grasp_transf,
tool_type, itj_tool_changer_urdf_path,
itj_g1_urdf_path, viewer, diagnosis):
# modified from https://github.com/yijiangh/pybullet_planning/blob/dev/tests/test_collisions.py
urdf_filename, semantics = abb_irb4600_40_255_setup
move_group = 'bare_arm'
ee_touched_link_names = ['link_6']
with PyChoreoClient(viewer=viewer) as client:
with LockRenderer():
robot = client.load_robot(urdf_filename)
robot.semantics = semantics
client.disabled_collisions = robot.semantics.disabled_collisions
if tool_type == 'static':
for _, ee_cm in itj_TC_g1_cms.items():
client.add_collision_mesh(ee_cm)
else:
client.add_tool_from_urdf('TC', itj_tool_changer_urdf_path)
client.add_tool_from_urdf('g1', itj_g1_urdf_path)
# * add static obstacles
client.add_collision_mesh(base_plate_cm)
client.add_collision_mesh(column_obstacle_cm)
ik_joint_names = robot.get_configurable_joint_names(group=move_group)
ik_joint_types = robot.get_joint_types_by_names(ik_joint_names)
flange_link_name = robot.get_end_effector_link_name(group=move_group)
tool0_tf = Transformation.from_frame(
client.get_link_frame_from_name(robot, flange_link_name))
tool0_from_tool_changer_base = itj_tool_changer_grasp_transf
tool0_from_gripper_base = itj_gripper_grasp_transf
client.set_object_frame(
'^{}'.format('TC'),
Frame.from_transformation(tool0_tf * tool0_from_tool_changer_base))
client.set_object_frame(
'^{}'.format('g1'),
Frame.from_transformation(tool0_tf * tool0_from_gripper_base))
names = client._get_collision_object_names('^{}'.format('g1')) + \
client._get_collision_object_names('^{}'.format('TC'))
for ee_name in names:
attach_options = {'robot': robot}
if tool_type == 'actuated':
attached_child_link_name = 'toolchanger_base' if 'TC' in ee_name else 'gripper_base'
attach_options.update(
{'attached_child_link_name': attached_child_link_name})
client.add_attached_collision_mesh(AttachedCollisionMesh(
CollisionMesh(None, ee_name),
flange_link_name,
touch_links=ee_touched_link_names),
options=attach_options)
# client._print_object_summary()
# wait_if_gui('EE attached.')
if tool_type == 'actuated':
# lower 0.0008 upper 0.01
tool_bodies = client._get_bodies('^{}'.format('itj_PG500'))
tool_conf = Configuration(
values=[0.01, 0.01],
types=[Joint.PRISMATIC, Joint.PRISMATIC],
joint_names=['joint_gripper_jaw_l', 'joint_gripper_jaw_r'])
for b in tool_bodies:
client._set_body_configuration(b, tool_conf)
wait_if_gui('Open')
tool_conf = Configuration(
values=[0.0008, 0.0008],
types=[Joint.PRISMATIC, Joint.PRISMATIC],
joint_names=['joint_gripper_jaw_l', 'joint_gripper_jaw_r'])
for b in tool_bodies:
client._set_body_configuration(b, tool_conf)
wait_if_gui('Close')
cprint('safe start conf', 'green')
conf = Configuration(values=[0.] * 6,
types=ik_joint_types,
joint_names=ik_joint_names)
assert not client.check_collisions(
robot, conf, options={'diagnosis': diagnosis})
cprint('joint over limit', 'cyan')
conf = Configuration(values=[0., 0., 1.5, 0, 0, 0],
types=ik_joint_types,
joint_names=ik_joint_names)
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
cprint('attached gripper-obstacle collision - column', 'cyan')
vals = [
-0.33161255787892263, -0.43633231299858238, 0.43633231299858238,
-1.0471975511965976, 0.087266462599716474, 0.0
]
# conf = Configuration(values=vals, types=ik_joint_types, joint_names=ik_joint_names)
# client.set_robot_configuration(robot, conf)
# wait_if_gui()
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
#* attach beam
client.add_collision_mesh(itj_beam_cm)
tool0_tf = Transformation.from_frame(
client.get_link_frame_from_name(robot, flange_link_name))
tool0_from_beam_base = itj_beam_grasp_transf
client.set_object_frame(
'^{}$'.format('itj_beam_b2'),
Frame.from_transformation(tool0_tf * tool0_from_beam_base))
client.add_attached_collision_mesh(AttachedCollisionMesh(
CollisionMesh(None, 'itj_beam_b2'),
flange_link_name,
touch_links=[]),
options={'robot': robot})
# wait_if_gui('beam attached.')
cprint('attached beam-robot body self collision', 'cyan')
vals = [
0.73303828583761843, -0.59341194567807209, 0.54105206811824214,
-0.17453292519943295, 1.064650843716541, 1.7278759594743862
]
conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
cprint('attached beam-obstacle collision - column', 'cyan')
vals = [
0.087266462599716474, -0.19198621771937624, 0.20943951023931956,
0.069813170079773182, 1.2740903539558606, 0.069813170079773182
]
conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
cprint('attached beam-obstacle collision - ground', 'cyan')
vals = [
-0.017453292519943295, 0.6108652381980153, 0.20943951023931956,
1.7627825445142729, 1.2740903539558606, 0.069813170079773182
]
conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
cprint('robot link-obstacle collision - column', 'cyan')
vals = [
-0.41887902047863912, 0.20943951023931956, 0.20943951023931956,
1.7627825445142729, 1.2740903539558606, 0.069813170079773182
]
conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
cprint('robot link-obstacle collision - ground', 'cyan')
vals = [
0.33161255787892263, 1.4660765716752369, 0.27925268031909273,
0.17453292519943295, 0.22689280275926285, 0.54105206811824214
]
conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert client.check_collisions(robot,
conf,
options={'diagnosis': diagnosis})
cprint('Sweeping collision', 'cyan')
vals = [
-0.12217304763960307, -0.73303828583761843, 0.83775804095727824,
-2.4609142453120048, 1.2391837689159739, -0.85521133347722145
]
conf1 = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert not client.check_collisions(
robot, conf1, options={'diagnosis': diagnosis})
# wait_if_gui()
vals = [
-0.12217304763960307, -0.73303828583761843, 0.83775804095727824,
-2.4958208303518914, -1.5533430342749532, -0.85521133347722145
]
conf2 = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
assert not client.check_collisions(
robot, conf2, options={'diagnosis': diagnosis})
# wait_if_gui()
assert client.check_sweeping_collisions(robot,
conf1,
conf2,
options={
'diagnosis': diagnosis,
'line_width': 3.0
})
wait_if_gui("Finished.")
def current_transformation(self):
if self.origin:
return Transformation.from_frame(self.origin)
else:
return Transformation()
def get_tool0_transformation(self, T5):
"""Get the transformation to reach tool0_frame.
"""
return T5 * Transformation.from_frame(self.tool0_frame)
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 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 test_inverse():
f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
T = Transformation.from_frame(f)
assert Transformation() == T * T.inverse()
def current_transformation(self):
"""Current transformation of the joint."""
if self.origin:
return Transformation.from_frame(self.origin)
else:
return Transformation()
def transformation_from_pose(pose, scale=1.0):
frame = frame_from_pose(pose, scale)
return Transformation.from_frame(frame)
def update_base_transformation(self, base_frame):
self.base_transformation = Transformation.from_frame(base_frame).inverse()
def test_plan_motion(abb_irb4600_40_255_setup, itj_TC_g1_cms, itj_beam_cm,
column_obstacle_cm, base_plate_cm,
itj_tool_changer_grasp_transf, itj_gripper_grasp_transf,
itj_beam_grasp_transf, tool_type,
itj_tool_changer_urdf_path, itj_g1_urdf_path, viewer,
diagnosis):
# modified from https://github.com/yijiangh/pybullet_planning/blob/dev/tests/test_collisions.py
urdf_filename, semantics = abb_irb4600_40_255_setup
move_group = 'bare_arm'
ee_touched_link_names = ['link_6']
with PyChoreoClient(viewer=viewer) as client:
with LockRenderer():
robot = client.load_robot(urdf_filename)
robot.semantics = semantics
client.disabled_collisions = robot.semantics.disabled_collisions
if tool_type == 'static':
for _, ee_cm in itj_TC_g1_cms.items():
client.add_collision_mesh(ee_cm)
else:
client.add_tool_from_urdf('TC', itj_tool_changer_urdf_path)
client.add_tool_from_urdf('g1', itj_g1_urdf_path)
# * add static obstacles
client.add_collision_mesh(base_plate_cm)
client.add_collision_mesh(column_obstacle_cm)
ik_joint_names = robot.get_configurable_joint_names(group=move_group)
ik_joint_types = robot.get_joint_types_by_names(ik_joint_names)
flange_link_name = robot.get_end_effector_link_name(group=move_group)
tool0_tf = Transformation.from_frame(
client.get_link_frame_from_name(robot, flange_link_name))
tool0_from_tool_changer_base = itj_tool_changer_grasp_transf
tool0_from_gripper_base = itj_gripper_grasp_transf
client.set_object_frame(
'^{}'.format('TC'),
Frame.from_transformation(tool0_tf * tool0_from_tool_changer_base))
client.set_object_frame(
'^{}'.format('g1'),
Frame.from_transformation(tool0_tf * tool0_from_gripper_base))
names = client._get_collision_object_names('^{}'.format('g1')) + \
client._get_collision_object_names('^{}'.format('TC'))
for ee_name in names:
attach_options = {'robot': robot}
if tool_type == 'actuated':
attached_child_link_name = 'toolchanger_base' if 'TC' in ee_name else 'gripper_base'
attach_options.update(
{'attached_child_link_name': attached_child_link_name})
client.add_attached_collision_mesh(AttachedCollisionMesh(
CollisionMesh(None, ee_name),
flange_link_name,
touch_links=ee_touched_link_names),
options=attach_options)
#* attach beam
client.add_collision_mesh(itj_beam_cm)
tool0_tf = Transformation.from_frame(
client.get_link_frame_from_name(robot, flange_link_name))
tool0_from_beam_base = itj_beam_grasp_transf
client.set_object_frame(
'^{}$'.format('itj_beam_b2'),
Frame.from_transformation(tool0_tf * tool0_from_beam_base))
client.add_attached_collision_mesh(AttachedCollisionMesh(
CollisionMesh(None, 'itj_beam_b2'),
flange_link_name,
touch_links=[]),
options={'robot': robot})
wait_if_gui('beam attached.')
vals = [
-1.4660765716752369, -0.22689280275926285, 0.27925268031909273,
0.17453292519943295, 0.22689280275926285, -0.22689280275926285
]
start_conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
# client.set_robot_configuration(robot, start_conf)
# wait_if_gui()
# vals = [0.05235987755982989, -0.087266462599716474, -0.05235987755982989, 1.7104226669544429, 0.13962634015954636, -0.43633231299858238]
vals = [
0.034906585039886591, 0.68067840827778847, 0.15707963267948966,
-0.89011791851710809, -0.034906585039886591, -2.2514747350726849
]
end_conf = Configuration(values=vals,
types=ik_joint_types,
joint_names=ik_joint_names)
# client.set_robot_configuration(robot, end_conf)
# wait_if_gui()
plan_options = {'diagnosis': True, 'resolutions': 0.05}
goal_constraints = robot.constraints_from_configuration(
end_conf, [0.01], [0.01], group=move_group)
st_time = time.time()
trajectory = client.plan_motion(robot,
goal_constraints,
start_configuration=start_conf,
group=move_group,
options=plan_options)
print('Solving time: {}'.format(elapsed_time(st_time)))
if trajectory is None:
cprint('Client motion planner CANNOT find a plan!', 'red')
# assert False, 'Client motion planner CANNOT find a plan!'
# TODO warning
else:
cprint('Client motion planning find a plan!', 'green')
wait_if_gui('Start sim.')
time_step = 0.03
for traj_pt in trajectory.points:
client.set_robot_configuration(robot, traj_pt)
wait_for_duration(time_step)
wait_if_gui("Finished.")
>>> T = Transformation.from_frame(frame)
>>> circle_transformed = cylinder.transformed(T)
"""
cylinder = self.copy()
cylinder.transform(transformation)
return cylinder
# ==============================================================================
# Main
# ==============================================================================
if __name__ == "__main__":
from compas.geometry import Transformation
cylinder = Cylinder(Circle(Plane.worldXY(), 5), 7)
frame = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
print(frame.normal)
T = Transformation.from_frame(frame)
cylinder.transform(T)
print(cylinder)
print(Plane.worldXY().data)
data = {'circle': Circle(Plane.worldXY(), 5).data, 'height': 7.}
cylinder = Cylinder.from_data(data)
print(cylinder)
import doctest
doctest.testmod()
def init_transformation(self):
if self.init_origin:
return Transformation.from_frame(self.init_origin)
else:
return Transformation()
