def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = 1.
trajectory.joint_names = response.trajectory.joint_trajectory.joint_names
trajectory.planning_time = response.planning_time
joint_types = [
joint_type_by_name[name] for name in trajectory.joint_names
]
trajectory.points = convert_trajectory_points(
response.trajectory.joint_trajectory.points, joint_types)
start_state = response.trajectory_start.joint_state
start_state_types = [
joint_type_by_name[name] for name in start_state.name
]
trajectory.start_configuration = Configuration(
start_state.position, start_state_types, start_state.name)
trajectory.attached_collision_meshes = list(
itertools.chain(*[
aco.to_attached_collision_meshes() for aco in
response.trajectory_start.attached_collision_objects
]))
callback(trajectory)
def trajectory_from_data(traj_data):
try:
return JointTrajectory.from_data(traj_data)
except AttributeError:
return [
Frame.from_data(frame_data) for frame_data in traj_data
]
def trj():
p1 = JointTrajectoryPoint([1.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.] * 6,
time_from_start=Duration(2, 1293))
p2 = JointTrajectoryPoint([0.571, 0, 0, 0.262, 0, 0], [0] * 6, [3.] * 6,
time_from_start=Duration(6, 0))
config = Configuration.from_revolute_values([0.] * 6)
return JointTrajectory(trajectory_points=[p1, p2],
joint_names=[
'joint_1', 'joint_2', 'joint_3', 'joint_4',
'joint_5', 'joint_6'
],
start_configuration=config)
def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = 1.
trajectory.points = convert_trajectory_points(
response.trajectory.joint_trajectory.points, joint_types)
trajectory.start_configuration = Configuration(
response.trajectory_start.joint_state.position,
start_configuration.types)
trajectory.planning_time = response.planning_time
callback(trajectory)
def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = 1.
trajectory.joint_names = response.trajectory.joint_trajectory.joint_names
trajectory.planning_time = response.planning_time
joint_types = robot.get_joint_types_by_names(trajectory.joint_names)
trajectory.points = convert_trajectory_points(
response.trajectory.joint_trajectory.points, joint_types)
start_state = response.trajectory_start.joint_state
start_state_types = robot.get_joint_types_by_names(start_state.name)
trajectory.start_configuration = Configuration(start_state.position, start_state_types)
callback(trajectory)
def convert_to_trajectory(response):
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = response.fraction
trajectory.points = convert_trajectory_points(
response.solution.joint_trajectory.points, joint_types)
trajectory.start_configuration = Configuration(
response.start_state.joint_state.position,
start_configuration.types)
callback(trajectory)
def data(self, data):
self.frame = Frame.from_data(data['frame'])
if '_tool_frame' in data:
self.tool_frame = Frame.from_data(data['_tool_frame'])
if '_source' in data:
self._source = _deserialize_from_data(data['_source'])
if '_mesh' in data:
#self._mesh = _deserialize_from_data(data['_mesh'])
self._mesh = Mesh.from_data(data['_mesh'])
if 'trajectory' in data:
from compas_fab.robots import JointTrajectory
self.trajectory = [
JointTrajectory.from_data(d) for d in data['trajectory']
]
#self.trajectory = _deserialize_from_data(data['trajectory'])
if 'path' in data:
self.path = [Frame.from_data(d) for d in data['path']]
def convert_trajectory(joints, solution, solution_start_state, fraction,
planning_time, source_message):
trajectory = JointTrajectory()
trajectory.source_message = source_message
trajectory.fraction = fraction
trajectory.joint_names = solution.joint_trajectory.joint_names
trajectory.planning_time = planning_time
joint_types = [joints[name] for name in trajectory.joint_names]
trajectory.points = convert_trajectory_points(
solution.joint_trajectory.points, joint_types)
start_state = solution_start_state.joint_state
start_state_types = [joints[name] for name in start_state.name]
trajectory.start_configuration = Configuration(start_state.position,
start_state_types,
start_state.name)
trajectory.attached_collision_meshes = list(
itertools.chain(*[
aco.to_attached_collision_meshes()
for aco in solution_start_state.attached_collision_objects
]))
return trajectory
def convert_to_trajectory(response):
try:
trajectory = JointTrajectory()
trajectory.source_message = response
trajectory.fraction = response.fraction
trajectory.joint_names = response.solution.joint_trajectory.joint_names
joint_types = [joint_type_by_name[name] for name in trajectory.joint_names]
trajectory.points = convert_trajectory_points(
response.solution.joint_trajectory.points, joint_types)
start_state = response.start_state.joint_state
start_state_types = [joint_type_by_name[name] for name in start_state.name]
trajectory.start_configuration = Configuration(start_state.position, start_state_types, start_state.name)
callback(trajectory)
except Exception as e:
errback(e)
def plan_motion(self, robot, goal_constraints, start_configuration=None, group=None, options=None):
"""Calculates a motion path.
Parameters
----------
robot : :class:`compas_fab.robots.Robot`
The robot instance for which the motion path is being calculated.
goal_constraints: list of :class:`compas_fab.robots.Constraint`
The goal to be achieved, defined in a set of constraints.
Constraints can be very specific, for example defining value domains
for each joint, such that the goal configuration is included,
or defining a volume in space, to which a specific robot link (e.g.
the end-effector) is required to move to.
start_configuration: :class:`compas_fab.robots.Configuration`, optional
The robot's full configuration, i.e. values for all configurable
joints of the entire robot, at the starting position.
group: str, optional
The name of the group to plan for.
options : dict, optional
Dictionary containing kwargs for arguments specific to
the client being queried.
- ``"avoid_collisions"``: (:obj:`bool`, optional)
Whether or not to avoid collisions. Defaults to ``True``.
- ``"max_step"``: (:obj:`float`, optional) The approximate distance between the
calculated points. (Defined in the robot's units.) Defaults to ``0.01``.
- ``"jump_threshold"``: (:obj:`float`, optional)
The maximum allowed distance of joint positions between consecutive
points. If the distance is found to be above this threshold, the
path computation fails. It must be specified in relation to max_step.
If this threshold is ``0``, 'jumps' might occur, resulting in an invalid
cartesian path. Defaults to :math:`\\pi / 2`.
Returns
-------
:class:`compas_fab.robots.JointTrajectory`
The calculated trajectory.
"""
robot_uid = robot.attributes['pybullet_uid']
# * parse options
verbose = is_valid_option(options, 'verbose', False)
diagnosis = options.get('diagnosis', False)
custom_limits = options.get('custom_limits') or {}
resolutions = options.get('resolutions') or 0.1
weights = options.get('weights') or None
rrt_restarts = options.get('rrt_restarts', 2)
rrt_iterations = options.get('rrt_iterations', 20)
smooth_iterations = options.get('smooth_iterations', 20)
# TODO: auto compute joint weight
# print('plan motion options: ', options)
# * convert link/joint names to pybullet indices
joint_names = robot.get_configurable_joint_names(group=group)
ik_joints = joints_from_names(robot_uid, joint_names)
joint_types = robot.get_joint_types_by_names(joint_names)
pb_custom_limits = get_custom_limits(robot_uid, ik_joints,
custom_limits={joint_from_name(robot_uid, jn) : lims for jn, lims in custom_limits.items()})
# print('pb custom limits: ', list(pb_custom_limits))
with WorldSaver():
if start_configuration is not None:
# * set to start conf
self.client.set_robot_configuration(robot, start_configuration)
sample_fn = get_sample_fn(robot_uid, ik_joints, custom_limits=pb_custom_limits)
distance_fn = get_distance_fn(robot_uid, ik_joints, weights=weights)
extend_fn = get_extend_fn(robot_uid, ik_joints, resolutions=resolutions)
options['robot'] = robot
collision_fn = PyChoreoConfigurationCollisionChecker(self.client)._get_collision_fn(robot, joint_names, options=options)
start_conf = get_joint_positions(robot_uid, ik_joints)
end_conf = self._joint_values_from_joint_constraints(joint_names, goal_constraints)
assert len(ik_joints) == len(end_conf)
if not check_initial_end(start_conf, end_conf, collision_fn, diagnosis=diagnosis):
return None
path = birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn, collision_fn,
restarts=rrt_restarts, iterations=rrt_iterations, smooth=smooth_iterations)
#return plan_lazy_prm(start_conf, end_conf, sample_fn, extend_fn, collision_fn)
if path is None:
# TODO use LOG
if verbose:
cprint('No free motion found!', 'red')
return None
else:
jt_traj_pts = []
for i, conf in enumerate(path):
# c_conf = Configuration(values=conf, types=joint_types, joint_names=joint_names)
jt_traj_pt = JointTrajectoryPoint(values=conf, types=joint_types, time_from_start=Duration(i*1,0))
# TODO why don't we have a `joint_names` input for JointTrajectoryPoint?
# https://github.com/compas-dev/compas_fab/blob/master/src/compas_fab/robots/trajectory.py#L64
jt_traj_pt.joint_names = joint_names
jt_traj_pts.append(jt_traj_pt)
trajectory = JointTrajectory(trajectory_points=jt_traj_pts,
joint_names=joint_names, start_configuration=jt_traj_pts[0], fraction=1.0)
return trajectory
def plan_cartesian_motion(self,
robot,
frames_WCF,
start_configuration=None,
group=None,
options=None):
"""Calculates a cartesian motion path (linear in tool space).
Parameters
----------
robot : :class:`compas_fab.robots.Robot`
The robot instance for which the motion path is being calculated.
frames_WCF: list of :class:`compas.geometry.Frame`
The frames through which the path is defined.
start_configuration: :class:`Configuration`, optional
The robot's full configuration, i.e. values for all configurable
joints of the entire robot, at the starting position.
group: str, optional
The planning group used for calculation.
options: dict, optional
Dictionary containing kwargs for arguments specific to
the client being queried.
- ``"diagnosis"``: (:obj:`bool`, optional)
. Defaults to ``False``.
- ``"avoid_collisions"``: (:obj:`bool`, optional)
Whether or not to avoid collisions. Defaults to ``True``.
- ``"planner_id"``: (:obj:`str`)
The name of the algorithm used for path planning.
Defaults to ``'IterativeIK'``.
Available planners: ``'IterativeIK', 'LadderGraph'``
- ``"max_step"``: (:obj:`float`, optional) The approximate distance between the
calculated points. (Defined in the robot's units.) Defaults to ``0.01``.
- ``"jump_threshold"``: (:obj:`float`, optional)
The maximum allowed distance of joint positions between consecutive
points. If the distance is found to be above this threshold, the
path computation fails. It must be specified in relation to max_step.
If this threshold is ``0``, 'jumps' might occur, resulting in an invalid
cartesian path.
Defaults to :math:`\\pi / 6` for revolute or continuous joints, 0.1 for prismatic joints.
# TODO: JointConstraint
Returns
-------
:class:`compas_fab.robots.JointTrajectory`
The calculated trajectory.
"""
robot_uid = robot.attributes['pybullet_uid']
# * convert link/joint names to pybullet indices
base_link_name = robot.get_base_link_name(group=group)
tool_link_name = robot.get_end_effector_link_name(group=group)
tool_link = link_from_name(robot_uid, tool_link_name)
base_link = link_from_name(robot_uid, base_link_name)
joint_names = robot.get_configurable_joint_names(group=group)
joint_types = robot.get_joint_types_by_names(joint_names)
ik_joints = joints_from_names(robot_uid, joint_names)
# * parse options
verbose = options.get('verbose', True)
diagnosis = options.get('diagnosis', False)
avoid_collisions = options.get('avoid_collisions', True)
pos_step_size = options.get('max_step', 0.01)
planner_id = is_valid_option(options, 'planner_id', 'IterativeIK')
jump_threshold = is_valid_option(options, 'jump_threshold', {jt_name : math.pi/6 \
if jt_type in [Joint.REVOLUTE, Joint.CONTINUOUS] else 0.1 \
for jt_name, jt_type in zip(joint_names, joint_types)})
jump_threshold_from_joint = {
joint_from_name(robot_uid, jt_name): j_diff
for jt_name, j_diff in jump_threshold.items()
}
# * iterative IK options
pos_tolerance = is_valid_option(options, 'pos_tolerance', 1e-3)
ori_tolerance = is_valid_option(options, 'ori_tolerance', 1e-3 * np.pi)
# * ladder graph options
frame_variant_gen = is_valid_option(options, 'frame_variant_generator',
None)
ik_function = is_valid_option(options, 'ik_function', None)
# * convert to poses and do workspace linear interpolation
given_poses = [pose_from_frame(frame_WCF) for frame_WCF in frames_WCF]
ee_poses = []
for p1, p2 in zip(given_poses[:-1], given_poses[1:]):
c_interp_poses = list(
interpolate_poses(p1, p2, pos_step_size=pos_step_size))
ee_poses.extend(c_interp_poses)
# * build collision fn
attachments = values_as_list(self.client.pychoreo_attachments)
collision_fn = PyChoreoConfigurationCollisionChecker(
self.client)._get_collision_fn(robot, joint_names, options=options)
failure_reason = ''
with WorldSaver():
# set to start conf
if start_configuration is not None:
self.client.set_robot_configuration(robot, start_configuration)
if planner_id == 'IterativeIK':
selected_links = [
link_from_name(robot_uid, l)
for l in robot.get_link_names(group=group)
]
# with HideOutput():
# with redirect_stdout():
path = plan_cartesian_motion_from_links(
robot_uid,
selected_links,
tool_link,
ee_poses,
get_sub_conf=False,
pos_tolerance=pos_tolerance,
ori_tolerance=ori_tolerance)
if path is None:
failure_reason = 'IK plan is not found.'
# collision checking is not included in the default Cartesian planning
if path is not None and avoid_collisions:
for i, conf_val in enumerate(path):
pruned_conf_val = self._prune_configuration(
robot_uid, conf_val, joint_names)
for attachment in attachments:
attachment.assign()
if collision_fn(pruned_conf_val, diagnosis=diagnosis):
failure_reason = 'IK plan is found but collision violated.'
path = None
break
path[i] = pruned_conf_val
# TODO check joint threshold
elif planner_id == 'LadderGraph':
# get ik fn from client
# collision checking is turned off because collision checking is handled inside LadderGraph planner
ik_options = {'avoid_collisions': False, 'return_all': True}
sample_ik_fn = ik_function or self._get_sample_ik_fn(
robot, ik_options)
# convert ee_variant_fn
if frame_variant_gen is not None:
def sample_ee_fn(pose):
for v_frame in frame_variant_gen.generate_frame_variant(
frame_from_pose(pose)):
yield pose_from_frame(v_frame)
else:
sample_ee_fn = None
path, cost = plan_cartesian_motion_lg(robot_uid, ik_joints, ee_poses, sample_ik_fn, collision_fn, \
jump_threshold=jump_threshold_from_joint, sample_ee_fn=sample_ee_fn)
if verbose:
print('Ladder graph cost: {}'.format(cost))
else:
raise ValueError('Cartesian planner {} not implemented!',
planner_id)
if path is None:
if verbose:
cprint(
'No Cartesian motion found, due to {}!'.format(
failure_reason), 'red')
return None
else:
# TODO start_conf might have different number of joints with the given group?
start_traj_pt = None
if start_configuration is not None:
start_traj_pt = JointTrajectoryPoint(
values=start_configuration.values,
types=start_configuration.types)
start_traj_pt.joint_names = start_configuration.joint_names
jt_traj_pts = []
for i, conf in enumerate(path):
jt_traj_pt = JointTrajectoryPoint(values=conf,
types=joint_types)
jt_traj_pt.joint_names = joint_names
if start_traj_pt is not None:
# ! TrajectoryPoint doesn't copy over joint_names...
jtp = start_traj_pt.copy()
jtp.joint_names = start_traj_pt.joint_names
jtp.merge(jt_traj_pt)
jt_traj_pt = jtp
jt_traj_pt.time_from_start = Duration(i * 1, 0)
jt_traj_pts.append(jt_traj_pt)
if start_configuration is not None and not compare_configurations(
start_configuration,
jt_traj_pts[0],
jump_threshold,
verbose=verbose):
# if verbose:
# print()
# cprint('Joint jump from start conf, max diff {}'.format(start_configuration.max_difference(jt_traj_pts[0])), 'red')
# cprint('start conf {}'.format(['{:.4f}'.format(v) for v in start_configuration.values]), 'red')
# cprint('traj pt 0 {}'.format(['{:.4f}'.format(v) for v in jt_traj_pts[0].values]), 'red')
pass
# return None
# TODO check intermediate joint jump
trajectory = JointTrajectory(
trajectory_points=jt_traj_pts,
joint_names=jt_traj_pts[0].joint_names,
start_configuration=jt_traj_pts[0],
fraction=1.0)
return trajectory
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 plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, group=None, options=None):
"""Calculates a cartesian motion path (linear in tool space).
Parameters
----------
robot : :class:`compas_fab.robots.Robot`
The robot instance for which the cartesian motion path is being calculated.
frames_WCF : list of :class:`compas.geometry.Frame`
The frames through which the path is defined.
start_configuration : :class:`Configuration`, optional
The robot's full configuration, i.e. values for all configurable
joints of the entire robot, at the starting position.
group : str, optional
The planning group used for calculation.
options : dict, optional
Dictionary containing kwargs for arguments specific to
the client being queried.
Returns
-------
:class:`compas_fab.robots.JointTrajectory`
The calculated trajectory.
Notes
-----
This will only work with robots that have 6 revolute joints.
"""
# what is the expected behaviour of that function?
# - Return all possible paths or select only the one that is closest to the start_configuration?
# - Do we use a stepsize to sample in between frames or use only the input frames?
# convert the frame WCF to RCF
base_frame = robot.get_base_frame(group=group, full_configuration=start_configuration)
frames_RCF = [base_frame.to_local_coordinates(frame_WCF) for frame_WCF in frames_WCF]
options = options or {}
options.update({"keep_order": True})
configurations_along_path = []
for frame in frames_RCF:
configurations = list(robot.iter_inverse_kinematics(frame, options=options))
configurations_along_path.append(configurations)
paths = []
for configurations in zip(*configurations_along_path):
if all(configurations):
paths.append(configurations)
if not len(paths):
raise CartesianMotionError("No complete trajectory found.")
# now select the path that is closest to the start configuration.
first_configurations = [path[0] for path in paths]
diffs = [sum([abs(d) for d in start_configuration.iter_differences(c)]) for c in first_configurations]
idx = argmin(diffs)
path = paths[idx]
path = self.smooth_configurations(path)
trajectory = JointTrajectory()
trajectory.fraction = len(path)/len(frames_RCF)
trajectory.joint_names = path[0].joint_names
trajectory.points = [JointTrajectoryPoint(config.joint_values, config.joint_types) for config in path]
trajectory.start_configuration = robot.merge_group_with_full_configuration(path[0], start_configuration, group)
return trajectory
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)
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_serialization(trj):
data = trj.to_data()
new_trj = JointTrajectory.from_data(data)
assert (new_trj.to_data() == data)
assert (new_trj.time_from_start == Duration(6, 0).seconds)
def traj_reparam(compas_fab_jt_traj,
max_jt_vel,
max_jt_acc,
traj_time_cnt=0,
ts_sample_num=100,
grid_num=200,
inspect_sol=False):
dof = len(compas_fab_jt_traj.points[0].values)
# Create path
way_jt_pts = [jt_pt.values for jt_pt in compas_fab_jt_traj.points]
path = ta.SplineInterpolator(
np.linspace(0, 1, len(compas_fab_jt_traj.points)), way_jt_pts)
# Create velocity bounds, then velocity constraint object
vlim_ = np.ones(6) * max_jt_vel
vlim = np.vstack((-vlim_, vlim_)).T
# Create acceleration bounds, then acceleration constraint object
alim_ = np.ones(6) * max_jt_acc
alim = np.vstack((-alim_, alim_)).T
pc_vel = constraint.JointVelocityConstraint(vlim)
pc_acc = constraint.JointAccelerationConstraint(
alim,
discretization_scheme=constraint.DiscretizationType.Interpolation)
# Setup a parametrization instance, then retime
gridpoints = np.linspace(0, path.duration, grid_num)
instance = algo.TOPPRA([pc_vel, pc_acc],
path,
gridpoints=gridpoints,
solver_wrapper='seidel')
jnt_traj, aux_traj, int_data = instance.compute_trajectory(
0, 0, return_data=True)
if inspect_sol:
ts_sample = np.linspace(0, jnt_traj.get_duration(), 100)
qdds_sample = jnt_traj.evaldd(ts_sample)
qds_sample = jnt_traj.evald(ts_sample)
fig, axs = plt.subplots(1, 2, sharex=True, figsize=[12, 4])
for i in range(6):
axs[0].plot(ts_sample,
qdds_sample[:, i],
label="J{:d}".format(i + 1))
axs[1].plot(ts_sample,
qds_sample[:, i],
label="J{:d}".format(i + 1))
axs[0].set_xlabel("Time (s)")
axs[0].set_ylabel("Joint acceleration (rad/s^2)")
axs[0].legend()
axs[1].legend()
axs[1].set_xlabel("Time (s)")
axs[1].set_ylabel("Joint velocity (rad/s)")
plt.show()
time_list = np.linspace(0, float(jnt_traj.get_duration()), ts_sample_num)
jt_list = jnt_traj.eval(time_list)
vel_list = jnt_traj.evald(time_list)
acc_list = jnt_traj.evaldd(time_list)
reparm_traj_pts = []
for i in range(ts_sample_num):
new_jt_pt = JointTrajectoryPoint(values=jt_list[i].tolist(),
types=[0] * 6)
new_jt_pt.velocities = vel_list[i].tolist()
new_jt_pt.accelerations = acc_list[i].tolist()
new_jt_pt.time_from_start = Duration.from_seconds(traj_time_cnt)
traj_time_cnt += time_list[i]
reparm_traj_pts.append(new_jt_pt)
if i == 0 and np.array_equal(time_list, np.zeros(ts_sample_num)):
break
reparam_traj = JointTrajectory(trajectory_points=reparm_traj_pts,
start_configuration=reparm_traj_pts[0])
return reparam_traj