def load_robot(self, urdf_file, resource_loaders=None):
"""Create a pybullet robot using the input urdf file.
Parameters
----------
urdf_file : :obj:`str` or file object
Absolute file path to the urdf file name or file object. The mesh file can be linked by either
`"package::"` or relative path.
resource_loaders : :obj:`list`
List of :class:`compas.robots.AbstractMeshLoader` for loading geometry of the robot. That the
geometry of the robot model is loaded is required before adding or removing attached collision meshes
to or from the scene. Defaults to the empty list.
Notes
-----
By default, PyBullet will use the convex hull of any mesh loaded from a URDF for collision detection.
Amending the link tag as ``<link concave="yes" name="<name of link>">`` will make the mesh concave
for static meshes (see this `example <https://github.com/bulletphysics/bullet3/blob/master/data/samurai.urdf>`_).
For non-static concave meshes, replace the OBJ files within the URDF with those generated by ``pybullet.vhacd``.
"""
robot_model = RobotModel.from_urdf_file(urdf_file)
robot = Robot(robot_model, client=self)
robot.attributes['pybullet'] = {}
if resource_loaders:
robot_model.load_geometry(*resource_loaders)
self.cache_robot(robot)
else:
robot.attributes['pybullet']['cached_robot'] = robot.model
robot.attributes['pybullet']['cached_robot_filepath'] = urdf_file
urdf_fp = robot.attributes['pybullet']['cached_robot_filepath']
self._load_robot_to_pybullet(urdf_fp, robot)
return robot
def get_picknplace_robot_data():
MODEL_DIR = coop_assembly.get_data('models')
robot_urdf = os.path.join(MODEL_DIR, ROBOT_URDF)
robot_srdf = os.path.join(MODEL_DIR, ROBOT_SRDF)
workspace_urdf = os.path.join(MODEL_DIR, WS_URDF)
workspace_srdf = os.path.join(MODEL_DIR, WS_SRDF)
move_group = None
robot_model = RobotModel.from_urdf_file(robot_urdf)
robot_semantics = RobotSemantics.from_srdf_file(robot_srdf, robot_model)
robot = RobotClass(robot_model, semantics=robot_semantics)
base_link_name = robot.get_base_link_name(group=move_group)
# ee_link_name = robot.get_end_effector_link_name(group=move_group)
ee_link_name = None # set to None since end effector is not included in the robot URDF, but attached later
ik_joint_names = robot.get_configurable_joint_names(group=move_group)
disabled_self_collision_link_names = robot_semantics.get_disabled_collisions(
)
tool_root_link_name = robot.get_end_effector_link_name(group=move_group)
workspace_model = RobotModel.from_urdf_file(workspace_urdf)
workspace_semantics = RobotSemantics.from_srdf_file(
workspace_srdf, workspace_model)
workspace_robot_disabled_link_names = workspace_semantics.get_disabled_collisions(
)
workspace_robot_disabled_link_names = []
return (robot_urdf, base_link_name, tool_root_link_name, ee_link_name, ik_joint_names, disabled_self_collision_link_names), \
(workspace_urdf, workspace_robot_disabled_link_names)
def load_robot(self, urdf_file):
"""Create a pybullet robot using the input urdf file.
Parameters
----------
urdf_file : :obj:`str` or file object
Absolute file path to the urdf file name or file object. The mesh file can be linked by either
`"package::"` or relative path.
Notes
-----
By default, PyBullet will use the convex hull of any mesh loaded from a URDF for collision detection.
Amending the link tag as ``<link concave="yes" name="<name of link>">`` will make the mesh concave
for static meshes (see this `example <https://github.com/bulletphysics/bullet3/blob/master/data/samurai.urdf>`_).
For non-static concave meshes, replace the OBJ files within the URDF with those generated by ``pybullet.vhacd``.
"""
robot_model = RobotModel.from_urdf_file(urdf_file)
robot = Robot(robot_model, client=self)
with redirect_stdout(enabled=not self.verbose):
pybullet_uid = pybullet.loadURDF(urdf_file, useFixedBase=True,
physicsClientId=self.client_id,
flags=pybullet.URDF_USE_SELF_COLLISION)
robot.attributes['pybullet_uid'] = pybullet_uid
self._add_ids_to_robot_joints(robot)
self._add_ids_to_robot_links(robot)
return robot
def Robot(client=None, load_geometry=False):
"""Returns a UR5 robot.
This method serves mainly as help for writing examples, so that the code can
stay short.
It is intentionally capitalized to act as an almost drop-in replacement for
the constructor of :class:`compas_fab.robots.Robot`.
Parameters
----------
client: object
Backend client.
load_geometry: bool
Returns
-------
:class:`compas_fab.robots.Robot`
Newly created instance of a UR5 robot.
Examples
--------
>>> from compas_fab.robots.ur5 import Robot
>>> robot = Robot()
>>> robot.name
'ur5'
"""
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')
model = RobotModel.from_urdf_file(urdf_filename)
semantics = RobotSemantics.from_srdf_file(srdf_filename, model)
if load_geometry:
loader = LocalPackageMeshLoader(compas_fab.get('universal_robot'),
'ur_description')
model.load_geometry(loader)
robot = RobotClass(model, semantics=semantics)
if client:
robot.client = client
return robot
def load_robot(self,
load_geometry=False,
urdf_param_name='/robot_description',
srdf_param_name='/robot_description_semantic',
precision=None,
local_cache_directory=None):
"""Load an entire robot instance -including model and semantics- directly from ROS.
Parameters
----------
load_geometry : bool, optional
``True`` to load the robot's geometry, otherwise ``False`` to load only the model and semantics.
urdf_param_name : str, optional
Parameter name where the URDF is defined. If not defined, it will default to ``/robot_description``.
srdf_param_name : str, optional
Parameter name where the SRDF is defined. If not defined, it will default to ``/robot_description_semantic``.
precision : float
Defines precision for importing/loading meshes. Defaults to ``compas.PRECISION``.
local_cache_directory : str, optional
Directory where the robot description (URDF, SRDF and meshes) are stored.
This differs from the directory taken as parameter by the :class:`RosFileServerLoader`
in that it points directly to the specific robot package, not to a global workspace storage
for all robots. If not assigned, the robot will not be cached locally.
Examples
--------
>>> from compas_fab.backends import RosClient
>>> with RosClient() as client:
... robot = client.load_robot()
... print(robot.name)
ur5
"""
robot_name = None
use_local_cache = False
if local_cache_directory is not None:
use_local_cache = True
path_parts = local_cache_directory.strip(os.path.sep).split(
os.path.sep)
path_parts, robot_name = path_parts[:-1], path_parts[-1]
local_cache_directory = os.path.sep.join(path_parts)
loader = RosFileServerLoader(self, use_local_cache,
local_cache_directory, precision)
if robot_name:
loader.robot_name = robot_name
urdf = loader.load_urdf(urdf_param_name)
srdf = loader.load_srdf(srdf_param_name)
model = RobotModel.from_urdf_string(urdf)
semantics = RobotSemantics.from_srdf_string(srdf, model)
if load_geometry:
model.load_geometry(loader)
return Robot(model, semantics=semantics, client=self)
def load_ur5(self, load_geometry=False, concavity=False):
""""Load a UR5 robot to PyBullet.
Parameters
----------
load_geometry : :obj:`bool`, optional
Indicate whether to load the geometry of the robot or not.
concavity : :obj:`bool`, optional
When ``False`` (the default), the mesh will be loaded as its
convex hull for collision checking purposes. When ``True``,
a non-static mesh will be decomposed into convex parts using v-HACD.
Returns
-------
:class:`compas_fab.robots.Robot`
A robot instance.
"""
robot_model = RobotModel.ur5(load_geometry)
robot = Robot(robot_model, client=self)
robot.attributes['pybullet'] = {}
if load_geometry:
self.cache_robot(robot, concavity)
else:
robot.attributes['pybullet']['cached_robot'] = robot.model
robot.attributes['pybullet']['cached_robot_filepath'] = compas.get(
'ur_description/urdf/ur5.urdf')
urdf_fp = robot.attributes['pybullet']['cached_robot_filepath']
self._load_robot_to_pybullet(urdf_fp, robot)
srdf_filename = compas_fab.get(
'universal_robot/ur5_moveit_config/config/ur5.srdf')
self.load_semantics(robot, srdf_filename)
return robot
def test_basic_name_links(ur5_links):
robot = Robot.basic('testbot', links=ur5_links)
assert len(robot.model.links) == 11
def test_basic_name_joints_links(ur5_joints, ur5_links):
robot = Robot.basic('testbot', joints=ur5_joints, links=ur5_links)
assert len(robot.model.links) == 11
assert len(robot.get_configurable_joint_names()) == 6
def test_basic_name_joints(ur5_joints):
robot = Robot.basic('testbot', joints=ur5_joints)
assert len(robot.model.joints) == 10
def sequenced_picknplace_plan(
assembly_json_path,
robot_model='ur3',
pick_from_same_rack=True,
customized_sequence=[],
from_seq_id=0,
to_seq_id=None,
num_cart_steps=10,
enable_viewer=True,
plan_transit=True,
transit_res=0.01,
view_ikfast=False,
tcp_tf_list=[1e-3 * 80.525, 0, 0],
robot_start_conf=[0, -1.65715, 1.71108, -1.62348, 0, 0],
scale=1,
result_save_path='',
sim_traj=True,
cart_ts=0.1,
trans_ts=0.01,
per_conf_step=False,
step_sim=False):
# parser.add_argument('-vik', '--view_ikfast', action='store_true', help='Visualize each ikfast solutions')
# parser.add_argument('-per_conf_step', '--per_conf_step', action='store_true', help='stepping each configuration in simulation')
# transition motion planner settings
RRT_RESTARTS = 5
RRT_ITERATIONS = 40
# rescaling
# TODO: this should be done when the Assembly object is made
unit_geos, static_obstacles = load_assembly_package(assembly_json_path,
scale=scale)
# urdf, end effector settings
if robot_model == '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/' +
'dms_2019_gripper/collision/190907_Gripper_05.obj')
# 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)
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()
disabled_link_names = semantics.get_disabled_collisions()
# parse end effector mesh
ee_meshes = [Mesh.from_obj(ee_filename)]
tcp_tf = Translation(tcp_tf_list)
# add static collision obstacles
co_dict = {}
for i, static_obs_mesh in enumerate(static_obstacles):
cm = CollisionMesh(static_obs_mesh, 'so_' + str(i))
co_dict[cm.id] = {}
co_dict[cm.id]['meshes'] = [cm.mesh]
co_dict[cm.id]['mesh_poses'] = [cm.frame]
# ======================================================
# ======================================================
# start pybullet environment & load pybullet robot
connect(use_gui=enable_viewer)
camera_base_pt = (0, 0, 0)
camera_pt = np.array(camera_base_pt) + np.array([1, 0, 0.5])
set_camera_pose(tuple(camera_pt), camera_base_pt)
pb_robot = create_pb_robot_from_ros_urdf(urdf_filename,
urdf_pkg_name,
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)
set_joint_positions(pb_robot, pb_ik_joints, robot_start_conf)
for e_at in ee_attachs:
e_at.assign()
# draw TCP frame in pybullet
handles = []
if has_gui() and view_ikfast:
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
so_lists_from_name = convert_meshes_and_poses_to_pybullet_bodies(co_dict)
static_obstacles_from_name = {}
for so_key, so_val in so_lists_from_name.items():
for so_i, so_item in enumerate(so_val):
static_obstacles_from_name[so_key + '_' + str(so_i)] = so_item
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 = customized_sequence or list(range(len(unit_geos)))
element_seq = {seq_id: e_id for seq_id, e_id in enumerate(seq_assignment)}
to_seq_id = to_seq_id or len(element_seq) - 1
assert 0 <= from_seq_id and from_seq_id < len(element_seq)
assert from_seq_id <= to_seq_id and to_seq_id < len(element_seq)
if has_gui():
for e_id in element_seq.values():
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 robot_model == '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,
from_seq_id=from_seq_id, to_seq_id=to_seq_id,
pick_from_same_rack=pick_from_same_rack,
tcp_transf=pb_pose_from_Transformation(tcp_tf),
ee_attachs=ee_attachs, disabled_collision_link_names=disabled_link_names, viz=view_ikfast, st_conf=robot_start_conf,
num_cart_steps=num_cart_steps)
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, robot_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)
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_transit:
print('Transition planning started.')
disabled_collision_links = [(link_from_name(pb_robot, pair[0]), link_from_name(pb_robot, pair[1])) \
for pair in disabled_link_names]
for seq_id in range(0, from_seq_id):
e_id = element_seq[seq_id]
for e_body in unit_geos[e_id].pybullet_bodies:
set_pose(e_body, unit_geos[e_id].goal_pb_pose)
for seq_id in range(from_seq_id, to_seq_id + 1):
e_id = element_seq[seq_id]
print('----\ntransition seq#{} element #{}'.format(seq_id, e_id))
if seq_id != from_seq_id:
tr_start_conf = picknplace_cart_plans[
seq_id - 1 - from_seq_id]['place_retreat'][-1]
else:
tr_start_conf = robot_start_conf
place2pick_st_conf = list(tr_start_conf)
assert picknplace_cart_plans[seq_id - from_seq_id][
'pick_approach'], 'pick approach not found in sequence {} (element id: {})!'.format(
seq_id, e_id)
place2pick_goal_conf = list(
picknplace_cart_plans[seq_id -
from_seq_id]['pick_approach'][0])
saved_world = WorldSaver()
set_joint_positions(pb_robot, pb_ik_joints, place2pick_st_conf)
for ee_a in ee_attachs:
ee_a.assign()
built_obstacles = []
ignored_pairs = []
if pick_from_same_rack:
built_obstacles = flatten_unit_geos_bodies({element_seq[prev_seq_id] : \
unit_geos[element_seq[prev_seq_id]] for prev_seq_id in range(seq_id)})
# if seq_id > 0:
# ignored_pairs = list(product([ee_attach.child for ee_attach in ee_attachs], unit_geos[element_seq[seq_id-1]].pybullet_bodies))
else:
built_obstacles = flatten_unit_geos_bodies(unit_geos)
place2pick_obstacles = list(
static_obstacles_from_name.values()) + built_obstacles
place2pick_path = plan_joint_motion(
pb_robot,
pb_ik_joints,
place2pick_goal_conf,
attachments=ee_attachs,
obstacles=place2pick_obstacles,
disabled_collisions=disabled_collision_links,
self_collisions=True,
resolutions=[transit_res] * len(pb_ik_joints),
restarts=RRT_RESTARTS,
iterations=RRT_ITERATIONS,
ignored_pairs=ignored_pairs)
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(pb_robot, pb_ik_joints, \
obstacles=place2pick_obstacles,
attachments=ee_attachs, self_collisions=True, diagnosis=True)
print('start pose:')
cfn(place2pick_st_conf)
print('end pose:')
cfn(place2pick_goal_conf)
saved_world.restore()
print('Diagnosis over')
assert picknplace_cart_plans[seq_id - from_seq_id][
'pick_retreat'], 'pick retreat not found! in sequence {} (element id: {})!'.format(
seq_id, e_id)
assert picknplace_cart_plans[seq_id - from_seq_id][
'place_approach'], 'place approach not found! in sequence {} (element id: {})!'.format(
seq_id, e_id)
pick2place_st_conf = picknplace_cart_plans[
seq_id - from_seq_id]['pick_retreat'][-1]
pick2place_goal_conf = picknplace_cart_plans[
seq_id - from_seq_id]['place_approach'][0]
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 - from_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()
built_obstacles = []
if pick_from_same_rack:
built_obstacles = flatten_unit_geos_bodies({element_seq[prev_seq_id] : \
unit_geos[element_seq[prev_seq_id]] for prev_seq_id in range(seq_id)})
else:
built_obstacles = flatten_unit_geos_bodies(unit_geos)
pick2place_obstacles = list(
static_obstacles_from_name.values()) + built_obstacles
pick2place_path = plan_joint_motion(
pb_robot,
pb_ik_joints,
pick2place_goal_conf,
disabled_collisions=disabled_collision_links,
obstacles=pick2place_obstacles,
attachments=ee_attachs + element_attachs,
self_collisions=True,
resolutions=[transit_res] * 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(pb_robot, pb_ik_joints,
obstacles=pick2place_obstacles, \
attachments=ee_attachs + element_attachs, self_collisions=True, diagnosis=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 -
from_seq_id]['place2pick'] = place2pick_path
picknplace_cart_plans[seq_id -
from_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 == to_seq_id:
saved_world = WorldSaver()
return2idle_st_conf = picknplace_cart_plans[
seq_id - from_seq_id]['place_retreat'][-1]
return2idle_goal_conf = robot_start_conf
set_joint_positions(pb_robot, pb_ik_joints,
return2idle_st_conf)
for ee_a in ee_attachs:
ee_a.assign()
built_obstacles = flatten_unit_geos_bodies({element_seq[prev_seq_id] : \
unit_geos[element_seq[prev_seq_id]] for prev_seq_id in range(seq_id+1)})
return2idle_obstacles = list(
static_obstacles_from_name.values()) + built_obstacles
return2idle_path = plan_joint_motion(
pb_robot,
pb_ik_joints,
return2idle_goal_conf,
disabled_collisions=disabled_collision_links,
obstacles=return2idle_obstacles,
attachments=ee_attachs,
self_collisions=True,
resolutions=[transit_res] * len(pb_ik_joints),
restarts=RRT_RESTARTS,
iterations=RRT_ITERATIONS,
)
if not return2idle_path:
saved_world = WorldSaver()
print('****\nseq #{} cannot find return2idle transition'.
format(seq_id))
print('Diagnosis...')
cfn = get_collision_fn(pb_robot, pb_ik_joints, \
obstacles=return2idle_obstacles,
attachments=ee_attachs, self_collisions=True, diagnosis=True)
print('start pose:')
cfn(return2idle_st_conf)
print('end pose:')
cfn(return2idle_goal_conf)
saved_world.restore()
print('Diagnosis over')
saved_world.restore()
picknplace_cart_plans[
seq_id - from_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 = []
if not sub_process:
continue
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:
if not os.path.exists(os.path.dirname(result_save_path)):
os.mkdir(os.path.dirname(result_save_path))
with open(result_save_path, 'w+') as outfile:
json.dump(traj_json_data, outfile)
print('planned trajectories saved to {}'.format(result_save_path))
print('\n*************************\nplanning completed.')
if sim_traj and has_gui():
# 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, \
element_seq=element_seq,
from_seq_id=from_seq_id, to_seq_id=to_seq_id,
ee_attachs=ee_attachs,
cartesian_time_step=cart_ts,
transition_time_step=trans_ts, step_sim=step_sim, per_conf_step=per_conf_step)
return traj_json_data
def test_basic_name_only():
robot = Robot.basic('testbot')
assert robot.artist is None
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()
from compas.robots import RobotModel
import compas_fab
from compas_fab.backends import RosClient
from compas_fab.robots import Robot
with RosClient() as client:
urdf = compas_fab.get('universal_robot/ur_description/urdf/ur5.urdf')
model = RobotModel.from_urdf_file(urdf)
robot = Robot(model, client=client)
robot.info()
assert robot.name == 'ur5'
DATA = os.path.join(HERE, '../data')
PATH = os.path.join(DATA, 'robot_description')
#package = 'ur_description'
package = 'ur_setups'
urdf_filename = os.path.join(PATH, package, "urdf",
"ur10_with_measurement_tool.urdf")
srdf_filename = os.path.join(PATH, package, "ur10_with_measurement_tool.srdf")
#package = "abb_linear_axis"
#urdf_filename = os.path.join(PATH, package, "urdf", "abb_linear_axis_brick_suction_tool.urdf")
#srdf_filename = os.path.join(PATH, package, "abb_linear_axis_suction_tool.srdf")
model = RobotModel.from_urdf_file(urdf_filename)
loaders = []
loaders.append(LocalPackageMeshLoader(PATH, "ur_description"))
loaders.append(LocalPackageMeshLoader(PATH, "ur_end_effectors"))
#loaders.append(LocalPackageMeshLoader(PATH, "abb_linear_axis"))
#loaders.append(LocalPackageMeshLoader(PATH, "abb_irb4600_40_255"))
#loaders.append(LocalPackageMeshLoader(PATH, "abb_end_effectors"))
model.load_geometry(*loaders)
artist = None
# artist = RobotArtist(model)
semantics = RobotSemantics.from_srdf_file(srdf_filename, model)
robot = Robot(model, artist, semantics, client=None)
robot.info()
from compas_fab.backends import RosClient
from compas_fab.backends import RosFileServerLoader
from compas_fab.robots import Configuration
from compas_fab.robots import Robot
from compas_fab.robots import RobotSemantics
with RosClient() as client:
loader = RosFileServerLoader(client)
urdf = loader.load_urdf()
srdf = loader.load_srdf()
model = RobotModel.from_urdf_string(urdf)
semantics = RobotSemantics.from_srdf_string(srdf, model)
robot = Robot(model, semantics=semantics, client=client)
group = robot.main_group_name
frames = []
frames.append(Frame((0.2925, 0.3949, 0.5066), (0, 1, 0), (0, 0, 1)))
frames.append(Frame((0.5103, 0.2827, 0.4074), (0, 1, 0), (0, 0, 1)))
start_configuration = Configuration.from_revolute_values(
(0.667, -0.298, 0.336, -2.333, -1.787, 2.123, 0.571))
trajectory = robot.plan_cartesian_motion(frames,
start_configuration,
max_step=0.01,
avoid_collisions=True,
group=group)
from compas.robots import RobotModel
from compas_fab.backends import RosClient
from compas_fab.backends import RosFileServerLoader
from compas_fab.robots import Configuration
from compas_fab.robots import Robot
from compas_fab.robots import RobotSemantics
with RosClient() as client:
loader = RosFileServerLoader(client)
urdf = loader.load_urdf()
srdf = loader.load_srdf()
model = RobotModel.from_urdf_string(urdf)
semantics = RobotSemantics.from_srdf_string(srdf, model)
robot = Robot(model, semantics=semantics, client=client)
configuration = Configuration.from_prismatic_and_revolute_values(
[0.], [-2.238, -1.153, -2.174, 0.185, 0.667, 0.])
frame_RCF = robot.forward_kinematics(configuration)
frame_WCF = robot.to_world_coords(frame_RCF)
print("Frame in the robot's coordinate system")
print(frame_RCF)
print("Frame in the world coordinate system")
print(frame_WCF)
from compas_fab.robots import RobotSemantics
from compas_fab.artists import BaseRobotArtist
from compas_fab.backends import RosClient
from compas_fab.backends import RosError
compas.PRECISION = '12f'
HERE = os.path.dirname(__file__)
DATA = os.path.join(HERE, '../data')
PATH = os.path.join(DATA, 'robot_description')
# packages = ['abb_irb4600_40_255', 'abb_linear_axis', 'abb_end_effectors']
# loaders = [LocalPackageMeshLoader(PATH, package) for package in packages]
urdf_filename = "abb_linear_axis_brick_suction_tool.urdf"
srdf_filename = "abb_linear_axis_suction_tool.srdf"
package = "abb_linear_axis"
urdf_filename = os.path.join(PATH, package, "urdf", urdf_filename)
srdf_filename = os.path.join(PATH, package, srdf_filename)
model = RobotModel.from_urdf_file(urdf_filename)
# model.load_geometry(loader)
artist = BaseRobotArtist(model)
# artist = RobotArtist(model)
semantics = RobotSemantics.from_srdf_file(srdf_filename, model)
robot = Robot(model, artist, semantics)
if __name__ == '__main__':
robot.info()
def local_executor(cls, options, host='127.0.0.1', port=19997):
with VrepClient(debug=options.get('debug', True), host=host, port=port) as client:
active_robot_options = None
# Setup all robots' start state
for r in options['robots']:
robot = Robot(r['robot'], client)
if 'start' in r:
if r['start'].get('joint_values'):
start = Configuration.from_data(r['start'])
elif r['start'].get('values'):
start = Frame.from_data(r['start'])
try:
reachable_state = client.inverse_kinematics(robot, start, metric_values=[0.] * robot.dof, max_trials=1, max_results=1)
start = reachable_state[-1]
LOG.info('Robot state found for start pose. External axes=%s, Joint values=%s', str(start.external_axes), str(start.joint_values))
except VrepError:
raise ValueError('Start plane is not reachable: %s' % str(r['start']))
client.set_robot_config(robot, start)
if 'building_member' in r:
client.add_building_member(robot, Mesh.from_data(r['building_member']))
if 'goal' in r:
active_robot_options = r
# Set global scene options
if 'collision_meshes' in options:
client.add_meshes(map(Mesh.from_data, options['collision_meshes']))
# Check if there's at least one active robot (i.e. one with a goal defined)
if active_robot_options:
robot = Robot(active_robot_options['robot'], client)
if active_robot_options['goal'].get('values'):
goal = Pose.from_data(active_robot_options['goal'])
else:
raise ValueError('Unsupported goal type: %s' % str(active_robot_options['goal']))
kwargs = {}
kwargs['metric_values'] = active_robot_options.get('metric_values')
kwargs['planner_id'] = options.get('planner_id')
kwargs['resolution'] = options.get('resolution')
if 'joint_limits' in active_robot_options:
joint_limits = active_robot_options['joint_limits']
if joint_limits.get('gantry'):
kwargs['gantry_joint_limits'] = [item for sublist in joint_limits.get('gantry') for item in sublist]
if joint_limits.get('arm'):
kwargs['arm_joint_limits'] = [item for sublist in joint_limits.get('arm') for item in sublist]
kwargs['trials'] = options.get('trials')
kwargs['shallow_state_search'] = options.get('shallow_state_search')
kwargs['optimize_path_length'] = options.get('optimize_path_length')
# Filter None values
kwargs = {k: v for k, v in kwargs.iteritems() if v is not None}
path = client.plan_motion(robot, goal, **kwargs)
LOG.info('Found path of %d steps', len(path))
else:
robot = Robot(options['robots'][0]['robot'], client)
path = [client.get_robot_config(robot)]
return path
def single_place_check(seq_id,
assembly_json_path,
customized_sequence=[],
num_cart_steps=10,
robot_model='ur3',
enable_viewer=True,
view_ikfast=False,
tcp_tf_list=[1e-3 * 80.525, 0, 0],
scale=1,
diagnosis=False,
viz_time_gap=0.5):
# # rescaling
# # TODO: this should be done when the Assembly object is made
unit_geos, static_obstacle_meshes = load_assembly_package(
assembly_json_path, scale=scale)
assert seq_id < len(unit_geos)
if customized_sequence:
unit_geo = unit_geos[customized_sequence[seq_id]]
else:
unit_geo = unit_geos[seq_id]
# urdf, end effector settings
if robot_model == 'ur3':
urdf_filename = compas_fab.get(
'universal_robot/ur_description/urdf/ur3.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/' +
'dms_2019_gripper/collision/190907_Gripper_05.obj')
# 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)
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()
disabled_link_names = semantics.get_disabled_collisions()
# parse end effector mesh
ee_meshes = [Mesh.from_obj(ee_filename)]
tcp_tf = Translation(tcp_tf_list)
# ======================================================
# ======================================================
# start pybullet environment & load pybullet robot
connect(use_gui=enable_viewer)
camera_base_pt = (0, 0, 0)
camera_pt = np.array(camera_base_pt) + np.array([1, 0, 0.5])
set_camera_pose(tuple(camera_pt), camera_base_pt)
pb_robot = create_pb_robot_from_ros_urdf(urdf_filename,
urdf_pkg_name,
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)
robot_start_conf = [0, -1.65715, 1.71108, -1.62348, 0, 0]
set_joint_positions(pb_robot, pb_ik_joints, robot_start_conf)
for e_at in ee_attachs:
e_at.assign()
# viz handles
handles = []
# convert mesh into pybullet bodies
# TODO: this conversion should be moved into UnitGeometry
geo_bodies = []
for mesh in unit_geo.mesh:
geo_bodies.append(convert_mesh_to_pybullet_body(mesh))
unit_geo.pybullet_bodies = geo_bodies
static_obstacles = []
for so_mesh in static_obstacle_meshes:
static_obstacles.append(convert_mesh_to_pybullet_body(so_mesh))
if customized_sequence:
assembled_element_ids = [
customized_sequence[seq_iter] for seq_iter in range(seq_id)
]
else:
assembled_element_ids = list(range(seq_id))
print('assembled_ids: ', assembled_element_ids)
for prev_e_id in assembled_element_ids:
other_unit_geo = unit_geos[prev_e_id]
print('other unit geo: ', other_unit_geo)
for _, mesh in enumerate(other_unit_geo.mesh):
pb_e = convert_mesh_to_pybullet_body(mesh)
set_pose(pb_e, other_unit_geo.goal_pb_pose)
static_obstacles.append(pb_e)
# check collision between obstacles and element geometries
# assert not sanity_check_collisions([unit_geo], static_obstacles_from_name)
ik_fn = ikfast_ur3.get_ik if robot_model == 'ur3' else ikfast_ur5.get_ik
return quick_check_place_feasibility(
pb_robot,
ik_joint_names,
base_link_name,
ee_link_name,
ik_fn,
unit_geo,
num_cart_steps=num_cart_steps,
static_obstacles=static_obstacles,
self_collisions=True,
mount_link_from_tcp_pose=pb_pose_from_Transformation(tcp_tf),
ee_attachs=ee_attachs,
viz=view_ikfast,
disabled_collision_link_names=disabled_link_names,
diagnosis=diagnosis,
viz_time_gap=viz_time_gap)
def viz_saved_traj(traj_save_path,
assembly_json_path,
element_seq,
from_seq_id,
to_seq_id,
cart_ts=0.01,
trans_ts=0.01,
scale=0.001,
robot_model='ur3',
per_conf_step=False,
step_sim=False):
assert os.path.exists(traj_save_path) and os.path.exists(
assembly_json_path)
# rescaling
# TODO: this should be done when the Assembly object is made
unit_geos, static_obstacle_meshes = load_assembly_package(
assembly_json_path, scale=scale)
# urdf, end effector settings
if robot_model == '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/' +
'dms_2019_gripper/collision/190907_Gripper_05.obj')
model = RobotModel.from_urdf_file(urdf_filename)
semantics = RobotSemantics.from_srdf_file(srdf_filename, model)
robot = RobotClass(model, semantics=semantics)
ik_joint_names = robot.get_configurable_joint_names()
ee_link_name = robot.get_end_effector_link_name()
connect(use_gui=True)
camera_base_pt = (0, 0, 0)
camera_pt = np.array(camera_base_pt) + np.array([1, 0, 0.5])
set_camera_pose(tuple(camera_pt), camera_base_pt)
pb_robot = create_pb_robot_from_ros_urdf(urdf_filename,
urdf_pkg_name,
ee_link_name=ee_link_name)
ee_meshes = [Mesh.from_obj(ee_filename)]
ee_attachs = attach_end_effector_geometry(ee_meshes, pb_robot,
ee_link_name)
static_obstacles = []
for so_mesh in static_obstacle_meshes:
static_obstacles.append(convert_mesh_to_pybullet_body(so_mesh))
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
if os.path.exists(traj_save_path):
with open(traj_save_path, 'r') as f:
traj_json_data = json.loads(f.read())
for e_id in element_seq:
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, \
element_seq=element_seq,
from_seq_id=from_seq_id, to_seq_id=to_seq_id,
ee_attachs=ee_attachs,
cartesian_time_step=cart_ts,
transition_time_step=trans_ts, step_sim=step_sim, per_conf_step=per_conf_step)
else:
print('no saved traj found at: ', traj_save_path)
def test_basic_attr():
robot = Robot.basic('testbot', location="rfl")
assert robot.model.attr['location'] == "rfl"
def panda_robot_instance(panda_urdf, panda_srdf):
model = RobotModel.from_urdf_file(panda_urdf)
semantics = RobotSemantics.from_srdf_file(panda_srdf, model)
return Robot(model, semantics=semantics)
def test_ikfast_inverse_kinematics():
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'
model = RobotModel.from_urdf_file(urdf_filename)
semantics = RobotSemantics.from_srdf_file(srdf_filename, model)
robot = RobotClass(model, semantics=semantics)
base_link_name = robot.get_base_link_name()
ik_joint_names = robot.get_configurable_joint_names()
ik_tool_link_name = robot.get_end_effector_link_name()
connect(use_gui=False)
pb_robot = create_pb_robot_from_ros_urdf(urdf_filename, urdf_pkg_name)
pb_ik_joints = joints_from_names(pb_robot, ik_joint_names)
max_attempts = 20
EPS = 1e-3
sample_fn = get_sample_fn(pb_robot, pb_ik_joints)
for i in range(max_attempts):
# randomly sample within joint limits
conf = sample_fn()
# sanity joint limit violation check
assert not violates_limits(pb_robot, pb_ik_joints, conf)
# ikfast's FK
fk_fn = ikfast_ur5.get_fk
ikfast_FK_pb_pose = get_ik_tool_link_pose(fk_fn, pb_robot, ik_joint_names, base_link_name, conf)
if has_gui():
print('test round #{}: ground truth conf: {}'.format(i, conf))
handles = draw_pose(ikfast_FK_pb_pose, length=0.04)
set_joint_positions(pb_robot, pb_ik_joints, conf)
wait_for_user()
# ikfast's IK
ik_fn = ikfast_ur5.get_ik
ik_sols = sample_tool_ik(ik_fn, pb_robot, ik_joint_names, base_link_name,
ikfast_FK_pb_pose, get_all=True)
# TODO: UR robot or in general joint w/ domain over 4 pi
# needs specialized distance function
q_selected = sample_tool_ik(ik_fn, pb_robot, ik_joint_names, base_link_name,
ikfast_FK_pb_pose, nearby_conf=True)
qsol = best_sol(ik_sols, conf, [1.]*6)
print('q selected: {}'.format(q_selected))
print('q best: {}'.format(qsol))
if has_gui():
set_joint_positions(pb_robot, pb_ik_joints, qsol)
wait_for_user()
for h in handles : remove_debug(h)
if qsol is None:
qsol = [999.]*6
diff = np.sum(np.abs(np.array(qsol) - np.array(conf)))
if diff > EPS:
print(np.array(ik_sols))
print('Best q:{}'.format(qsol))
print('Actual:{}'.format(np.array(conf)))
print('Diff: {}'.format(conf - qsol))
print('Difdiv:{}'.format((conf - qsol)/np.pi))
assert False
def panda_robot_instance_wo_semantics(panda_urdf):
return Robot(RobotModel.from_urdf_file(panda_urdf), semantics=None)
from compas_fab.backends import RosClient
from compas_fab.backends import RosFileServerLoader
from compas_fab.robots import Configuration
from compas_fab.robots import Robot
from compas_fab.robots import RobotSemantics
with RosClient() as client:
loader = RosFileServerLoader(client)
urdf = loader.load_urdf()
srdf = loader.load_srdf()
model = RobotModel.from_urdf_string(urdf)
semantics = RobotSemantics.from_srdf_string(srdf, model)
robot = Robot(model, semantics=semantics, client=client)
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.667, -0.298, 0.336, -2.333, -1.787, 2.123, 0.571))
# 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,
from compas.robots import RobotModel
from compas_fab.backends import RosClient
from compas_fab.robots import Robot
with RosClient() as client:
model = RobotModel.ur5()
robot = Robot(model, client=client)
robot.info()
assert len(robot.get_configurable_joint_names()) == 6
from compas.geometry import Frame
from compas.robots import RobotModel
from compas_fab.backends import RosClient
from compas_fab.backends import RosFileServerLoader
from compas_fab.robots import Robot
from compas_fab.robots import RobotSemantics
with RosClient() as client:
loader = RosFileServerLoader(client)
urdf = loader.load_urdf()
srdf = loader.load_srdf()
model = RobotModel.from_urdf_string(urdf)
semantics = RobotSemantics.from_srdf_string(srdf, model)
robot = Robot(model, semantics=semantics, client=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)
def main():
choreo_problem_instance_dir = compas_fab.get('choreo_instances')
result_save_path = os.path.join(choreo_problem_instance_dir, 'results',
'choreo_result.json')
with open(result_save_path, 'r') as f:
json_data = json.loads(f.read())
traj_time_cnt = 0.0
max_jt_vel = 0.2
max_jt_acc = 0.1
last_jt_pt = None
# pybullet traj preview settings
pybullet_preview = True
PB_VIZ_CART_TIME_STEP = 0.05
PB_VIZ_TRANS_TIME_STEP = 0.04
PB_VIZ_PER_CONF_SIM = False
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')
# [0.0, -94.94770102010436, 98.0376624092449, -93.01855212389889, 0.0, 0.0]
# UR=192.168.0.30, Linux=192.168.0.1, Windows=192.168.0.2
# the following host IP should agree with the Linux machine
host_ip = '192.168.0.1' if REAL_EXECUTION else 'localhost'
with RosClient(host=host_ip, port=9090) as client:
client.on_ready(
lambda: print('Is ROS connected?', client.is_connected))
# get current configuration
listener = roslibpy.Topic(client, JOINT_TOPIC_NAME,
'sensor_msgs/JointState')
msg_getter = MsgGetter()
listener.subscribe(msg_getter.receive_msg)
time.sleep(2)
last_seen_state = msg_getter.get_msg()
print('current jt state: {}'.format(last_seen_state['position']))
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
joint_names = robot.get_configurable_joint_names()
# base_link_name = robot.get_base_link_name()
ee_link_name = robot.get_end_effector_link_name()
if pybullet_preview:
from conrob_pybullet import connect
from compas_fab.backends.ros.plugins_choreo import display_trajectory_chunk
from compas_fab.backends.pybullet import attach_end_effector_geometry, \
convert_mesh_to_pybullet_body, create_pb_robot_from_ros_urdf
connect(use_gui=True)
pb_robot = create_pb_robot_from_ros_urdf(urdf_filename,
urdf_pkg_name,
ee_link_name=ee_link_name)
ee_meshes = [Mesh.from_obj(ee_filename)]
ee_attachs = attach_end_effector_geometry(ee_meshes, pb_robot,
ee_link_name)
st_conf = Configuration.from_revolute_values(
last_seen_state['position'])
goal_conf = Configuration.from_revolute_values(
json_data[0]['place2pick']['start_configuration']['values'])
goal_constraints = robot.constraints_from_configuration(
goal_conf, [math.radians(1)] * 6, group)
init_traj_raw = robot.plan_motion(goal_constraints,
st_conf,
group,
planner_id='RRTStar')
init_traj = traj_reparam(init_traj_raw,
max_jt_vel,
max_jt_acc,
inspect_sol=False)
if pybullet_preview:
display_trajectory_chunk(pb_robot, joint_names,
init_traj.to_data(), \
ee_attachs=ee_attachs, grasped_attach=[],
time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)
print('************\nexecuting init transition')
exec_jt_traj(client, joint_names, init_traj)
print('executed?')
input()
for seq_id, e_process_data in enumerate(json_data):
print('************\nexecuting #{} picknplace process'.format(
seq_id))
# open gripper
gripper_srv_call(client, state=0)
print(
'=====\nexecuting #{} place-retreat to pick-approach transition process'
.format(seq_id))
traj_data = e_process_data['place2pick']
ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
ros_jt_traj = traj_reparam(ros_jt_traj_raw,
max_jt_vel,
max_jt_acc,
inspect_sol=False)
if pybullet_preview:
display_trajectory_chunk(pb_robot, joint_names,
ros_jt_traj.to_data(), \
ee_attachs=ee_attachs, grasped_attach=[],
time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)
exec_jt_traj(client, joint_names, ros_jt_traj)
print('=====\nexecuting #{} pick-approach to pick-grasp process'.
format(seq_id))
traj_data = e_process_data['pick_approach']
ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
ros_jt_traj = traj_reparam(ros_jt_traj_raw,
max_jt_vel,
max_jt_acc,
inspect_sol=False)
if pybullet_preview:
display_trajectory_chunk(pb_robot, joint_names,
ros_jt_traj.to_data(), \
ee_attachs=ee_attachs, grasped_attach=[],
time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)
exec_jt_traj(client, joint_names, ros_jt_traj)
print('executed?')
input()
# close gripper
gripper_srv_call(client, state=1)
print('=====\nexecuting #{} pick-grasp to pick-retreat process'.
format(seq_id))
traj_data = e_process_data['pick_retreat']
ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
ros_jt_traj = traj_reparam(ros_jt_traj_raw,
max_jt_vel,
max_jt_acc,
inspect_sol=False)
if pybullet_preview:
display_trajectory_chunk(pb_robot, joint_names,
ros_jt_traj.to_data(), \
ee_attachs=ee_attachs, grasped_attach=[],
time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)
exec_jt_traj(client, joint_names, ros_jt_traj)
print('executed?')
input()
print(
'=====\nexecuting #{} pick-retreat to place-approach transition process'
.format(seq_id))
traj_data = e_process_data['pick2place']
ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
ros_jt_traj = traj_reparam(ros_jt_traj_raw,
max_jt_vel,
max_jt_acc,
inspect_sol=False)
if pybullet_preview:
display_trajectory_chunk(pb_robot, joint_names,
ros_jt_traj.to_data(), \
ee_attachs=ee_attachs, grasped_attach=[],
time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)
exec_jt_traj(client, joint_names, ros_jt_traj)
print('executed?')
input()
print('=====\nexecuting #{} place-approach to place-grasp process'.
format(seq_id))
traj_data = e_process_data['place_approach']
ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
ros_jt_traj = traj_reparam(ros_jt_traj_raw,
max_jt_vel,
max_jt_acc,
inspect_sol=False)
if pybullet_preview:
display_trajectory_chunk(pb_robot, joint_names,
ros_jt_traj.to_data(), \
ee_attachs=ee_attachs, grasped_attach=[],
time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)
exec_jt_traj(client, joint_names, ros_jt_traj)
# open gripper
gripper_srv_call(client, state=0)
print('executed?')
input()
print('=====\nexecuting #{} place-grasp to place-retreat process'.
format(seq_id))
traj_data = e_process_data['place_retreat']
ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
ros_jt_traj = traj_reparam(ros_jt_traj_raw,
max_jt_vel,
max_jt_acc,
inspect_sol=False)
if pybullet_preview:
display_trajectory_chunk(pb_robot, joint_names,
ros_jt_traj.to_data(), \
ee_attachs=ee_attachs, grasped_attach=[],
time_step=PB_VIZ_CART_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)
exec_jt_traj(client, joint_names, ros_jt_traj)
print('executed?')
input()
if 'return2idle' in json_data[-1]:
print('=====\nexecuting #{} return-to-idle transition process'.
format(seq_id))
traj_data = e_process_data['return2idle']
ros_jt_traj_raw = JointTrajectory.from_data(traj_data)
ros_jt_traj = traj_reparam(ros_jt_traj_raw,
max_jt_vel,
max_jt_acc,
inspect_sol=False)
if pybullet_preview:
display_trajectory_chunk(pb_robot, joint_names,
ros_jt_traj.to_data(), \
ee_attachs=ee_attachs, grasped_attach=[],
time_step=PB_VIZ_TRANS_TIME_STEP, step_sim=True, per_conf_step=PB_VIZ_PER_CONF_SIM)
exec_jt_traj(client, joint_names, ros_jt_traj)