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 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 frame(self) -> compas.geometry.Frame:
location = self.shape.Location()
transformation = location.Transformation()
T = Transformation(
matrix=[[transformation.Value(i, j) for j in range(4)]
for i in range(4)])
frame = Frame.from_transformation(T)
return frame
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 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
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 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 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 main():
parser = argparse.ArgumentParser()
# ur_picknplace_multiple_piece
parser.add_argument('-p',
'--problem',
default='ur_picknplace_single_piece',
help='The name of the problem to solve')
parser.add_argument('-rob',
'--robot',
default='ur3',
help='The type of UR robot to use.')
parser.add_argument('-m',
'--plan_transit',
action='store_false',
help='Plans motions between each picking and placing')
parser.add_argument('-v',
'--viewer',
action='store_true',
help='Enables the viewer during planning (slow!)')
parser.add_argument('-s',
'--save_result',
action='store_true',
help='save planning results as a json file')
parser.add_argument(
'-scale',
'--model_scale',
default=0.001,
help='model scale conversion to meter, default 0.001 (from millimeter)'
)
parser.add_argument('-vik',
'--view_ikfast',
action='store_true',
help='Visualize each ikfast solutions')
parser.add_argument('-tres',
'--transit_res',
default=0.01,
help='joint resolution (rad)')
parser.add_argument('-ros',
'--use_ros',
action='store_true',
help='use ros backend with moveit planners')
parser.add_argument('-cart_ts',
'--cartesian_time_step',
default=0.1,
help='cartesian time step in trajectory simulation')
parser.add_argument('-trans_ts',
'--transit_time_step',
default=0.01,
help='transition time step in trajectory simulation')
parser.add_argument('-per_conf_step',
'--per_conf_step',
action='store_true',
help='stepping each configuration in simulation')
args = parser.parse_args()
print('Arguments:', args)
VIZ = args.viewer
VIZ_IKFAST = args.view_ikfast
TRANSITION_JT_RESOLUTION = float(args.transit_res)
plan_transition = args.plan_transit
use_moveit_planner = args.use_ros
# sim settings
CART_TIME_STEP = args.cartesian_time_step
TRANSITION_TIME_STEP = args.transit_time_step
PER_CONF_STEP = args.per_conf_step
# transition motion planner settings
RRT_RESTARTS = 5
RRT_ITERATIONS = 40
# choreo pkg settings
choreo_problem_instance_dir = compas_fab.get('choreo_instances')
unit_geos, static_obstacles = load_assembly_package(
choreo_problem_instance_dir, args.problem, scale=args.model_scale)
result_save_path = os.path.join(
choreo_problem_instance_dir, 'results',
'choreo_result.json') if args.save_result else None
# urdf, end effector settings
if args.robot == 'ur3':
# urdf_filename = compas_fab.get('universal_robot/ur_description/urdf/ur3.urdf')
urdf_filename = compas_fab.get(
'universal_robot/ur_description/urdf/ur3_collision_viz.urdf')
srdf_filename = compas_fab.get(
'universal_robot/ur3_moveit_config/config/ur3.srdf')
else:
urdf_filename = compas_fab.get(
'universal_robot/ur_description/urdf/ur5.urdf')
srdf_filename = compas_fab.get(
'universal_robot/ur5_moveit_config/config/ur5.srdf')
urdf_pkg_name = 'ur_description'
ee_filename = compas_fab.get(
'universal_robot/ur_description/meshes/' +
'pychoreo_workshop_gripper/collision/victor_gripper_jaw03.obj')
# ee_sep_filename = compas_fab.get('universal_robot/ur_description/meshes/' +
# 'pychoreo_workshop_gripper/collision/victor_gripper_jaw03_rough_sep.obj')
# ee_decomp_file_dir = compas_fab.get('universal_robot/ur_description/meshes/' +
# 'pychoreo_workshop_gripper/collision/decomp')
# ee_decomp_file_prefix = 'victor_gripper_jaw03_decomp_'
# decomp_parts_num = 36
client = RosClient() if use_moveit_planner else None
# geometry file is not loaded here
model = RobotModel.from_urdf_file(urdf_filename)
semantics = RobotSemantics.from_srdf_file(srdf_filename, model)
robot = RobotClass(model, semantics=semantics, client=client)
group = robot.main_group_name
base_link_name = robot.get_base_link_name()
ee_link_name = robot.get_end_effector_link_name()
ik_joint_names = robot.get_configurable_joint_names()
# parse end effector mesh
# ee_meshes = [Mesh.from_obj(os.path.join(ee_decomp_file_dir, ee_decomp_file_prefix + str(i) + '.obj')) for i in range(decomp_parts_num)]
ee_meshes = [Mesh.from_obj(ee_filename)]
# ee_meshes = [Mesh.from_obj(ee_sep_filename)]
# define TCP transformation
tcp_tf = Translation([0.099, 0, 0]) # in meters
ur5_start_conf = [0, -1.65715, 1.71108, -1.62348, 0, 0]
if use_moveit_planner:
# TODO: attach end effector to the robot in planning scene
# https://github.com/compas-dev/compas_fab/issues/66
scene = PlanningScene(robot)
scene.remove_all_collision_objects()
client.set_joint_positions(group, ik_joint_names, ur5_start_conf)
else:
scene = None
# add static collision obstacles
co_dict = {}
for i, static_obs_mesh in enumerate(static_obstacles):
# offset the table a bit...
cm = CollisionMesh(static_obs_mesh,
'so_' + str(i),
frame=Frame.from_transformation(
Translation([0, 0, -0.02])))
if use_moveit_planner:
scene.add_collision_mesh(cm)
else:
co_dict[cm.id] = {}
co_dict[cm.id]['meshes'] = [cm.mesh]
co_dict[cm.id]['mesh_poses'] = [cm.frame]
if use_moveit_planner:
# See: https://github.com/compas-dev/compas_fab/issues/63#issuecomment-519525879
time.sleep(1)
co_dict = scene.get_collision_meshes_and_poses()
# ======================================================
# ======================================================
# start pybullet environment & load pybullet robot
connect(use_gui=VIZ)
pb_robot = create_pb_robot_from_ros_urdf(urdf_filename,
urdf_pkg_name,
planning_scene=scene,
ee_link_name=ee_link_name)
ee_attachs = attach_end_effector_geometry(ee_meshes, pb_robot,
ee_link_name)
# update current joint conf and attach end effector
pb_ik_joints = joints_from_names(pb_robot, ik_joint_names)
pb_end_effector_link = link_from_name(pb_robot, ee_link_name)
if not use_moveit_planner:
set_joint_positions(pb_robot, pb_ik_joints, ur5_start_conf)
for e_at in ee_attachs:
e_at.assign()
# draw TCP frame in pybullet
if has_gui():
TCP_pb_pose = get_TCP_pose(pb_robot,
ee_link_name,
tcp_tf,
return_pb_pose=True)
handles = draw_pose(TCP_pb_pose, length=0.04)
# wait_for_user()
# deliver ros collision meshes to pybullet
static_obstacles_from_name = convert_meshes_and_poses_to_pybullet_bodies(
co_dict)
# for now...
for so_key, so_val in static_obstacles_from_name.items():
static_obstacles_from_name[so_key] = so_val[0]
for unit_name, unit_geo in unit_geos.items():
geo_bodies = []
for sub_id, mesh in enumerate(unit_geo.mesh):
geo_bodies.append(convert_mesh_to_pybullet_body(mesh))
unit_geo.pybullet_bodies = geo_bodies
# check collision between obstacles and element geometries
assert not sanity_check_collisions(unit_geos, static_obstacles_from_name)
# from random import shuffle
seq_assignment = list(range(len(unit_geos)))
# shuffle(seq_assignment)
element_seq = {seq_id: e_id for seq_id, e_id in enumerate(seq_assignment)}
# for key, val in element_seq.items():
# # element_seq[key] = 'e_' + str(val)
# element_seq[key] = val
if has_gui():
for e_id in element_seq.values():
# for e_body in brick_from_index[e_id].body: set_pose(e_body, brick_from_index[e_id].goal_pose)
handles.extend(
draw_pose(unit_geos[e_id].initial_pb_pose, length=0.02))
handles.extend(draw_pose(unit_geos[e_id].goal_pb_pose,
length=0.02))
for e_body in unit_geos[e_id].pybullet_bodies:
set_pose(e_body, unit_geos[e_id].initial_pb_pose)
print('pybullet env loaded.')
# wait_for_user()
for h in handles:
remove_debug(h)
saved_world = WorldSaver()
ik_fn = ikfast_ur3.get_ik if args.robot == 'ur3' else ikfast_ur5.get_ik
tot_traj, graph_sizes = \
direct_ladder_graph_solve_picknplace(pb_robot, ik_joint_names, base_link_name, ee_link_name, ik_fn,
unit_geos, element_seq, static_obstacles_from_name,
tcp_transf=pb_pose_from_Transformation(tcp_tf),
ee_attachs=ee_attachs,
max_attempts=100, viz=VIZ_IKFAST, st_conf=ur5_start_conf)
picknplace_cart_plans = divide_nested_list_chunks(tot_traj, graph_sizes)
saved_world.restore()
print('Cartesian planning finished.')
# reset robot and parts for better visualization
set_joint_positions(pb_robot, pb_ik_joints, ur5_start_conf)
for ee in ee_attachs:
ee.assign()
for e_id in element_seq.values():
for e_body in unit_geos[e_id].pybullet_bodies:
set_pose(e_body, unit_geos[e_id].initial_pb_pose)
# if has_gui():
# wait_for_user()
def flatten_unit_geos_bodies(in_dict):
out_list = []
for ug in in_dict.values():
out_list.extend(ug.pybullet_bodies)
return out_list
if plan_transition:
print('Transition planning started.')
for seq_id, unit_picknplace in enumerate(picknplace_cart_plans):
print('----\ntransition seq#{}'.format(seq_id))
e_id = element_seq[seq_id]
if seq_id != 0:
tr_start_conf = picknplace_cart_plans[seq_id -
1]['place_retreat'][-1]
else:
tr_start_conf = ur5_start_conf
# obstacles=static_obstacles + cur_mo_list
place2pick_st_conf = list(tr_start_conf)
place2pick_goal_conf = list(
picknplace_cart_plans[seq_id]['pick_approach'][0])
# assert not client.is_joint_state_colliding(group, ik_joint_names, place2pick_st_conf)
# assert not client.is_joint_state_colliding(group, ik_joint_names, place2pick_goal_conf)
if use_moveit_planner:
# TODO: add collision objects
st_conf = Configuration.from_revolute_values(
place2pick_st_conf)
goal_conf = Configuration.from_revolute_values(
place2pick_goal_conf)
goal_constraints = robot.constraints_from_configuration(
goal_conf, [math.radians(1)] * 6, group)
place2pick_jt_traj = robot.plan_motion(goal_constraints,
st_conf,
group,
planner_id='RRTConnect')
place2pick_path = [
jt_pt['values']
for jt_pt in place2pick_jt_traj.to_data()['points']
]
else:
saved_world = WorldSaver()
set_joint_positions(pb_robot, pb_ik_joints, place2pick_st_conf)
for ee_a in ee_attachs:
ee_a.assign()
place2pick_path = plan_joint_motion(
pb_robot,
pb_ik_joints,
place2pick_goal_conf,
attachments=ee_attachs,
obstacles=list(static_obstacles_from_name.values()) +
flatten_unit_geos_bodies(unit_geos),
self_collisions=True,
resolutions=[TRANSITION_JT_RESOLUTION] * len(pb_ik_joints),
restarts=RRT_RESTARTS,
iterations=RRT_ITERATIONS,
)
saved_world.restore()
if not place2pick_path:
saved_world = WorldSaver()
print('****\nseq #{} cannot find place2pick transition'.
format(seq_id))
print('Diagnosis...')
cfn = get_collision_fn_diagnosis(pb_robot, pb_ik_joints, \
obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos),
attachments=ee_attachs, self_collisions=True)
print('start pose:')
cfn(place2pick_st_conf)
print('end pose:')
cfn(place2pick_goal_conf)
saved_world.restore()
print('Diagnosis over')
pick2place_st_conf = picknplace_cart_plans[seq_id]['pick_retreat'][
-1]
pick2place_goal_conf = picknplace_cart_plans[seq_id][
'place_approach'][0]
if use_moveit_planner:
st_conf = Configuration.from_revolute_values(
picknplace_cart_plans[seq_id]['pick_retreat'][-1])
goal_conf = Configuration.from_revolute_values(
picknplace_cart_plans[seq_id]['place_approach'][0])
goal_constraints = robot.constraints_from_configuration(
goal_conf, [math.radians(1)] * 6, group)
pick2place_jt_traj = robot.plan_motion(goal_constraints,
st_conf,
group,
planner_id='RRTConnect')
pick2place_path = [
jt_pt['values']
for jt_pt in pick2place_jt_traj.to_data()['points']
]
else:
saved_world = WorldSaver()
# create attachement without needing to keep track of grasp...
set_joint_positions(
pb_robot, pb_ik_joints,
picknplace_cart_plans[seq_id]['pick_retreat'][0])
# attachs = [Attachment(robot, tool_link, invert(grasp.attach), e_body) for e_body in brick.body]
element_attachs = [create_attachment(pb_robot, pb_end_effector_link, e_body) \
for e_body in unit_geos[e_id].pybullet_bodies]
set_joint_positions(pb_robot, pb_ik_joints, pick2place_st_conf)
for ee_a in ee_attachs:
ee_a.assign()
for e_a in element_attachs:
e_a.assign()
pick2place_path = plan_joint_motion(
pb_robot,
pb_ik_joints,
pick2place_goal_conf,
obstacles=list(static_obstacles_from_name.values()) +
flatten_unit_geos_bodies(unit_geos),
attachments=ee_attachs + element_attachs,
self_collisions=True,
resolutions=[TRANSITION_JT_RESOLUTION] * len(pb_ik_joints),
restarts=RRT_RESTARTS,
iterations=RRT_ITERATIONS,
)
saved_world.restore()
if not pick2place_path:
saved_world = WorldSaver()
print('****\nseq #{} cannot find pick2place transition'.
format(seq_id))
print('Diagnosis...')
cfn = get_collision_fn_diagnosis(pb_robot, pb_ik_joints,
obstacles=list(static_obstacles_from_name.values()) + flatten_unit_geos_bodies(unit_geos), \
attachments=ee_attachs + element_attachs, self_collisions=True)
print('start pose:')
cfn(pick2place_st_conf)
print('end pose:')
cfn(pick2place_goal_conf)
saved_world.restore()
print('Diagnosis over')
picknplace_cart_plans[seq_id]['place2pick'] = place2pick_path
picknplace_cart_plans[seq_id]['pick2place'] = pick2place_path
for e_body in unit_geos[e_id].pybullet_bodies:
set_pose(e_body, unit_geos[e_id].goal_pb_pose)
if seq_id == len(picknplace_cart_plans) - 1:
saved_world = WorldSaver()
set_joint_positions(
pb_robot, pb_ik_joints,
picknplace_cart_plans[seq_id]['place_retreat'][-1])
for ee_a in ee_attachs:
ee_a.assign()
return2idle_path = plan_joint_motion(
pb_robot,
pb_ik_joints,
ur5_start_conf,
obstacles=list(static_obstacles_from_name.values()) +
flatten_unit_geos_bodies(unit_geos),
attachments=ee_attachs,
self_collisions=True,
resolutions=[TRANSITION_JT_RESOLUTION] * len(pb_ik_joints),
restarts=RRT_RESTARTS,
iterations=RRT_ITERATIONS,
)
saved_world.restore()
picknplace_cart_plans[seq_id]['return2idle'] = return2idle_path
print('Transition planning finished.')
# convert to ros JointTrajectory
traj_json_data = []
traj_time_count = 0.0
for i, element_process in enumerate(picknplace_cart_plans):
e_proc_data = {}
for sub_proc_name, sub_process in element_process.items():
sub_process_jt_traj_list = []
for jt_sol in sub_process:
sub_process_jt_traj_list.append(
JointTrajectoryPoint(values=jt_sol,
types=[0] * 6,
time_from_start=Duration(
traj_time_count, 0)))
traj_time_count += 1.0 # meaningless timestamp
e_proc_data[sub_proc_name] = JointTrajectory(
trajectory_points=sub_process_jt_traj_list,
start_configuration=sub_process_jt_traj_list[0]).to_data()
traj_json_data.append(e_proc_data)
if result_save_path:
with open(result_save_path, 'w+') as outfile:
json.dump(traj_json_data, outfile, indent=4)
print('planned trajectories saved to {}'.format(result_save_path))
print('\n*************************\nplanning completed. Simulate?')
if has_gui():
wait_for_user()
for e_id in element_seq.values():
for e_body in unit_geos[e_id].pybullet_bodies:
set_pose(e_body, unit_geos[e_id].initial_pb_pose)
display_picknplace_trajectories(pb_robot, ik_joint_names, ee_link_name,
unit_geos, traj_json_data, \
ee_attachs=ee_attachs,
cartesian_time_step=CART_TIME_STEP, transition_time_step=TRANSITION_TIME_STEP, step_sim=True, per_conf_step=PER_CONF_STEP)
if use_moveit_planner: scene.remove_all_collision_objects()
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 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.")
# ==============================================================================
if __name__ == "__main__":
from compas.geometry import Point
from compas.geometry import Frame
from compas.geometry import allclose
from compas.geometry import matrix_from_translation
from compas.geometry import matrix_from_scale_factors
from compas.geometry import transform_points
from numpy import asarray
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)
print(f1 == f2)
f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
T = Transformation.from_frame(f)
Tinv = T.inverse()
I = Transformation()
print(I == T * Tinv)
f1 = Frame([2, 2, 2], [0.12, 0.58, 0.81], [-0.80, 0.53, -0.26])
f2 = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
T = Transformation.from_frame_to_frame(f1, f2)
f1.transform(T)
print(f1 == f2)
f = Frame([1, 1, 1], [0.68, 0.68, 0.27], [-0.67, 0.73, -0.15])
def pose_from_transformation(tf, scale=1.0):
frame = Frame.from_transformation(tf)
return pose_from_frame(frame, scale)
