def plan_joint_motion(body,
joints,
end_conf,
obstacles=[],
attachments=[],
self_collisions=True,
disabled_collisions=set(),
weights=None,
resolutions=None,
max_distance=MAX_DISTANCE,
custom_limits={},
**kwargs):
assert len(joints) == len(end_conf)
sample_fn = get_sample_fn(body, joints, custom_limits=custom_limits)
distance_fn = get_distance_fn(body, joints, weights=weights)
extend_fn = get_extend_fn(body, joints, resolutions=resolutions)
collision_fn = get_collision_fn(body,
joints,
obstacles,
attachments,
self_collisions,
disabled_collisions,
custom_limits=custom_limits,
max_distance=max_distance)
start_conf = get_joint_positions(body, joints)
if not check_initial_end(start_conf, end_conf, collision_fn):
return None
return birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn,
collision_fn, **kwargs)
def plan_joint_motion(body,
joints,
end_conf,
obstacles=[],
attachments=[],
self_collisions=True,
disabled_collisions=set(),
extra_disabled_collisions=set(),
weights=None,
resolutions=None,
max_distance=MAX_DISTANCE,
ignore_collision_steps=0,
custom_limits={},
diagnosis=False,
constraint_fn=None,
max_dist_on=None,
**kwargs):
"""call birrt to plan a joint trajectory from the robot's **current** conf to ``end_conf``.
"""
assert len(joints) == len(end_conf)
sample_fn = get_sample_fn(body, joints, custom_limits=custom_limits)
distance_fn = get_distance_fn(body, joints, weights=weights)
extend_fn = get_extend_fn(body, joints, resolutions=resolutions)
collision_fn = get_collision_fn(
body,
joints,
obstacles=obstacles,
attachments=attachments,
self_collisions=self_collisions,
disabled_collisions=disabled_collisions,
extra_disabled_collisions=extra_disabled_collisions,
custom_limits=custom_limits,
max_distance=max_distance,
max_dist_on=max_dist_on)
if constraint_fn is not None:
old_collision_fn = collision_fn
def _collision_and_constraint(q, diagnosis=False):
return old_collision_fn(q, diagnosis) or constraint_fn(
q, diagnosis)
collision_fn = _collision_and_constraint
start_conf = get_joint_positions(body, joints)
if ignore_collision_steps == 0 and not check_initial_end(
start_conf, end_conf, collision_fn, diagnosis=diagnosis):
return None
return birrt(start_conf,
end_conf,
distance_fn,
sample_fn,
extend_fn,
collision_fn,
ignore_collision_steps=ignore_collision_steps,
**kwargs)
def plan_base_motion(body,
end_conf,
base_limits,
obstacles=[],
direct=False,
weights=1 * np.ones(3),
resolutions=0.05 * np.ones(3),
max_distance=MAX_DISTANCE,
**kwargs):
from pybullet_planning.interfaces.robots.collision import pairwise_collision
from pybullet_planning.interfaces.robots.body import set_base_values, get_base_values
def sample_fn():
x, y = np.random.uniform(*base_limits)
theta = np.random.uniform(*CIRCULAR_LIMITS)
return (x, y, theta)
difference_fn = get_base_difference_fn()
distance_fn = get_base_distance_fn(weights=weights)
def extend_fn(q1, q2):
steps = np.abs(np.divide(difference_fn(q2, q1), resolutions))
n = int(np.max(steps)) + 1
q = q1
for i in range(n):
q = tuple((1. / (n - i)) * np.array(difference_fn(q2, q)) + q)
yield q
# TODO: should wrap these joints
def collision_fn(q):
# TODO: update this function
set_base_values(body, q)
return any(
pairwise_collision(body, obs, max_distance=max_distance)
for obs in obstacles)
start_conf = get_base_values(body)
if collision_fn(start_conf):
print("Warning: initial configuration is in collision")
return None
if collision_fn(end_conf):
print("Warning: end configuration is in collision")
return None
if direct:
return direct_path(start_conf, end_conf, extend_fn, collision_fn)
return birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn,
collision_fn, **kwargs)
def plan_nonholonomic_motion(body,
joints,
end_conf,
obstacles=[],
attachments=[],
self_collisions=True,
disabled_collisions=set(),
weights=None,
resolutions=None,
reversible=True,
max_distance=MAX_DISTANCE,
custom_limits={},
**kwargs):
from pybullet_planning.interfaces.robots.joint import get_joint_positions
assert len(joints) == len(end_conf)
sample_fn = get_sample_fn(body, joints, custom_limits=custom_limits)
distance_fn = get_nonholonomic_distance_fn(body,
joints,
weights=weights,
reversible=reversible)
extend_fn = get_nonholonomic_extend_fn(body,
joints,
resolutions=resolutions,
reversible=reversible)
collision_fn = get_collision_fn(body,
joints,
obstacles,
attachments,
self_collisions,
disabled_collisions,
custom_limits=custom_limits,
max_distance=max_distance)
start_conf = get_joint_positions(body, joints)
if not check_initial_end(start_conf, end_conf, collision_fn):
return None
return birrt(start_conf, end_conf, distance_fn, sample_fn, extend_fn,
collision_fn, **kwargs)
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
