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_direct_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):
assert len(joints) == len(end_conf)
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 direct_path(start_conf,
end_conf,
extend_fn,
collision_fn,
ignore_collision_steps=ignore_collision_steps)
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_waypoints_joint_motion(body,
joints,
waypoints,
start_conf=None,
obstacles=[],
attachments=[],
self_collisions=True,
disabled_collisions=set(),
resolutions=None,
custom_limits={},
max_distance=MAX_DISTANCE):
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)
if start_conf is None:
start_conf = get_joint_positions(body, joints)
else:
assert len(start_conf) == len(joints)
for i, waypoint in enumerate([start_conf] + list(waypoints)):
if collision_fn(waypoint):
#print("Warning: waypoint configuration {}/{} is in collision".format(i, len(waypoints)))
return None
path = [start_conf]
for waypoint in waypoints:
assert len(joints) == len(waypoint)
for q in extend_fn(path[-1], waypoint):
if collision_fn(q):
return None
path.append(q)
return path
def get_collision_fn(self, config_type, diagnosis=False):
from pybullet_planning.interfaces.robots.collision import get_collision_fn
import functools
config = self.get_collision_conf(config_type)
return functools.partial(get_collision_fn(**config),
diagnosis=diagnosis)
def get_initial_conf(self, obs):
tips = obs['robot_tip_positions']
joints = obs['robot_position']
org_joint_conf = self.get_joint_conf()
num_trials = 20000
retry = 0
while retry < num_trials:
# reset joint configuration (neccesary because planner messes
# up with the pose)
self.env.platform.simfinger.reset_finger_positions_and_velocities(
org_joint_conf)
self.cube_tip_positions = sample_cube_surface_points(
cube_halfwidth=0.0500,
heuristic=self.heuristic,
shrink_region=0.5
)
target_tip_positions = apply_transform(obs['object_position'],
obs['object_orientation'],
self.cube_tip_positions)
if self.heuristic is 'center_of_two':
self.used_finger_ids = self.select_two_fingers(obs['goal_object_position'] - obs['object_position'])
_, inds = self.assign_positions_to_fingers(
tips[self.used_finger_ids, :],
target_tip_positions[:len(self.used_finger_ids)]
)
inds = self.used_finger_ids[list(inds)].tolist()
inds = inds + [3 - sum(inds)]
target_tip_positions = target_tip_positions[inds, :]
else:
target_tip_positions, inds = self.assign_positions_to_fingers(
tips, target_tip_positions)
self.cube_tip_positions = self.cube_tip_positions[inds, :]
# Run IK to get the target joint configuration
target_joint_conf = self.solve_for_joint_conf(target_tip_positions, joints)
if target_joint_conf is None:
continue
# Validate that the joint conf is reachable (IK solver sometimes returns an infeasible solution)
if not is_valid_action(target_joint_conf, action_type='position'):
# print('(initial conf) IK solver returned infeasible joint conf:', target_joint_conf)
continue
# Run motion planning to test the feasibility.
# Without this, sometimes fingers are initialized to go through the cube
planned_motion = plan_joint_motion(
self.env.platform.simfinger.finger_id,
self.env.platform.simfinger.pybullet_link_indices,
target_joint_conf,
self_collisions=True,
obstacles=[self.env.platform.cube.block],
diagnosis=False
)
# Test if the end pose is in collision
if planned_motion is not None:
obstacle = self._create_dummy_goal_object(obs) # dummy object for collision check
collision_fn = get_collision_fn(
self.env.platform.simfinger.finger_id,
self.env.platform.simfinger.pybullet_link_indices,
obstacles=[obstacle.block],
self_collisions=True,
max_distance=0
)
endpose = self._get_endpose(obs)
# Validate if endpose is reachable (IK solver sometimes returns an infeasible solution)
if endpose is not None and is_valid_action(endpose, action_type='position'):
endpose_in_collision = collision_fn(endpose, diagnosis=False)
# if endpose_in_collision:
# print('endpose is in collision')
else:
# print('IK solver returned infeasible joint conf:', endpose)
endpose_in_collision = True
del obstacle
if not endpose_in_collision:
break
retry += 1
# print('motion planning failed. retrying...\tcount:', retry)
if planned_motion is None:
raise RuntimeError('No feasible path to the target position is found.')
# reset joint configuration (neccesary because planner messes up with the pose)
self.env.platform.simfinger.reset_finger_positions_and_velocities(org_joint_conf)
# visualize sampled points
if self.env.visualization:
self.visual_markers.add(target_tip_positions, color=(0, 1, 1, 0.5))
return target_tip_positions, target_joint_conf
def _get_collision_fn(self, slacky_collision):
from pybullet_planning.interfaces.robots.collision import get_collision_fn
return get_collision_fn(**self._get_collision_conf(slacky_collision))