def plan_path(self, env, start, goal, vel=0.1):
"""
:rtype: np.array
"""
distance = lambda x, y: ((x[0] - y[0])**2 + (x[1] - y[1])**2)**0.5
def extend(last_config, s):
configs = [last_config.copy()]
curr_config = last_config.copy().astype(np.float32)
dt = env.dt_pose
diff = np.linalg.norm(last_config - s)
diff_comp = s - last_config
try:
num_steps_req = int(np.ceil(diff / (vel * dt)))
except:
import ipdb
ipdb.set_trace()
for i in range(num_steps_req):
curr_config[:] += diff_comp * dt
configs.append(curr_config.copy())
return configs
def sample():
return 0.1 * np.random.uniform((3, ))
def collision(pt):
ret = env.collision_fn(pt)
return ret
return birrt(start, goal, distance, sample, extend, collision)
def plan_joint_motion(body,
joints,
end_conf,
obstacles=None,
attachments=[],
self_collisions=True,
disabled_collisions=set(),
direct=False,
weights=None,
resolutions=None,
**kwargs):
# if direct:
# return plan_direct_joint_motion(body, joints, end_conf, obstacles, attachments, self_collisions, disabled_collisions)
assert len(joints) == len(end_conf)
sample_fn = get_sample_fn(body, joints)
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)
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_path(self, ne, start, goal):
"""
:rtype: np.array
"""
distance = lambda x, y: ((x[0] - y[0])**2 + (x[1] - y[1])**2)**0.5
dt = 0.01
# vel = 0.6+2*np.random.random()
vel = 0.9 + 2 * np.random.random()
def extend(last_config, s):
configs = [last_config.copy()]
curr_config = last_config.copy().astype(np.float32)
dt = 0.01
diff = np.linalg.norm(last_config - s)
diff_comp = s - last_config
num_steps_req = int(np.ceil(diff / (vel * dt)))
for i in range(num_steps_req):
curr_config[:] += diff_comp / num_steps_req
configs.append(curr_config.copy())
return configs
def sample():
x_rand = np.random.randint(low=0, high=ne.gridsize[0])
y_rand = np.random.randint(low=0, high=ne.gridsize[0])
return x_rand, y_rand
def collision(pt):
ret = ne.collision_fn(pt)
return ret
return birrt(start, goal, distance, sample, extend, collision)
def mp_birrt(robot, q1, q2, **kwargs):
use_mp()
from motion_planners.rrt_connect import birrt
env = robot.GetEnv()
with collision_saver(env, openravepy_int.CollisionOptions.ActiveDOFs):
# Excluding results in slow plans
return birrt(q1, q2, get_distance_fn(robot), get_sample_fn(env, robot),
get_extend_fn(robot), get_collision_fn(env, robot),
**kwargs)
def rrt(pos,
angle,
dpos,
dangle,
check_point_collision,
_,
motion_angle=0,
linear=False,
step_size=0.1,
iters=100,
restarts=2,
**kwargs):
'''
Return a RRT planned path from position pos, angle angle to position dpos, angle dangle.
'''
rotate_path1 = linear_rotate(pos, angle, motion_angle,
check_point_collision)
if rotate_path1 is None: return None
rotate_path2 = linear_rotate(dpos, motion_angle, dangle,
check_point_collision)
if rotate_path2 is None: return None
def sample_fn():
lower = [-SCREEN_WIDTH / 2., 0]
upper = [SCREEN_WIDTH / 2, SCREEN_HEIGHT - TABLE_HEIGHT]
return b2Vec2(np.random.uniform(lower, upper))
def distance_fn(q1, q2):
return np.linalg.norm(np.array(q2) - np.array(q1))
def extend_fn(q1, q2):
delta = np.array(q2) - np.array(q1)
distance = np.linalg.norm(delta)
for t in np.arange(step_size, distance, step_size):
yield q1 + (t * delta / distance)
yield q2
def collision_fn(q):
return check_point_collision(q, motion_angle)
if linear:
path = direct_path(pos, dpos, extend_fn, collision_fn)
else:
path = birrt(pos,
dpos,
distance_fn,
sample_fn,
extend_fn,
collision_fn,
restarts=restarts,
iterations=iters,
smooth=50)
if path is None:
return None
translate_path = np.array([np.hstack([pos, motion_angle]) for pos in path])
return np.vstack([rotate_path1[1:-1], translate_path, rotate_path2[1:-1]])
def sample_base_trajectory(robot, q1, q2, **kwargs):
# TODO: ActiveDOFs doesn't work here for FCL?
env = robot.GetEnv()
cspace = CSpace.robot_manipulator(robot, 'base')
cspace.name = 'base'
with robot:
cspace.set_active()
collision_fn = get_collision_fn(env,
robot,
self_collisions=False,
limits=True)
base_path = birrt(q1, q2, get_distance_fn(robot),
get_sample_fn(env, robot), get_extend_fn(robot),
collision_fn, **kwargs)
if base_path is None:
return None
return PathTrajectory(cspace, base_path)
def sample_free_motion(conf1, conf2): # Sample motion while not holding
if DISABLE_MOTION_TRAJECTORIES:
yield (Traj([conf2]), )
return
active_chains = [robot.armChainNames[ARM]]
path = birrt(conf1,
conf2,
get_distance_fn(active_chains),
get_sample_fn(conf1, active_chains),
get_extend_fn(active_chains, STEP_SIZE),
get_collision_fn(static_bodies),
restarts=0,
iterations=50,
smooth=None)
if path is None:
return
yield (Traj(path), )
def replan(self,
restarts=RRT_RESTARTS,
iterations=RRT_ITERATIONS,
smooth=RRT_SMOOTHING):
body = self.cspace.body
with body:
self.cspace.set_active()
path = birrt(self.start(),
self.end(),
distance_fn(body),
sample_fn(body),
extend_fn(body),
collision_fn(get_env(), body),
restarts=restarts,
iterations=iterations,
smooth=smooth)
if path is None: return None
return PathTrajectory(self.cspace, path)
def plan_base_motion(body,
end_conf,
obstacles=None,
direct=False,
base_limits=([-2.5, -2.5], [2.5, 2.5]),
weights=1 * np.ones(3),
resolutions=0.05 * np.ones(3),
**kwargs):
def sample_fn():
x, y = np.random.uniform(*base_limits)
theta = np.random.uniform(*CIRCULAR_LIMITS)
return (x, y, theta)
def difference_fn(q2, q1):
dx, dy = np.array(q2[:2]) - np.array(q1[:2])
dtheta = circular_difference(q2[2], q1[2])
return (dx, dy, dtheta)
def distance_fn(q1, q2):
difference = np.array(difference_fn(q2, q1))
return np.sqrt(np.dot(weights, difference * difference))
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):
set_base_values(body, q)
if obstacles is None:
return single_collision(body)
return any(pairwise_collision(body, obs) for obs in obstacles)
start_conf = get_base_values(body)
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 sample_holding_motion(conf1, conf2,
grasp): # Sample motion while holding
if DISABLE_MOTION_TRAJECTORIES:
yield (Traj([conf2], grasp), )
return
active_chains = [robot.armChainNames[ARM]]
attached = {
ARM: body.applyTrans(get_wrist_frame(grasp, robot, ARM).inverse())
}
path = birrt(conf1,
conf2,
get_distance_fn(active_chains),
get_sample_fn(conf1, active_chains),
get_extend_fn(active_chains, STEP_SIZE),
get_collision_fn(static_bodies, attached),
restarts=0,
iterations=50,
smooth=None)
if path is None:
return
yield (Traj(path, grasp), )
def mp_birrt(robot, q1, q2):
from motion_planners.rrt_connect import birrt
return birrt(q1, q2, get_distance_fn(robot),
get_sample_fn(robot.GetEnv(), robot), get_extend_fn(robot),
get_collision_fn(robot.GetEnv(), robot))