def obstacle_collision():
"""Tests whether an arm in its final configuration makes contact
with either of our obstacles.
"""
# Create two obstacles
obstacles = [Obstacle([1, 4, 1], [4, 5, 1.5]),
Obstacle([1, 2, 1], [4, 3, 1.5])]
# And a ball
ball = Ball([2.5, 2.5, 2.0], 0.25)
# Create an arm
q0 = np.array([0, 0, 0, np.pi / 2, 0, 0, 0])
human_arm = simple_human_arm(2.0, 2.0, q0, np.array([3.0, 1.0, 0.0]))
# Define environment
env = Environment(dimensions=[10.0, 10.0, 20.0],
dynamic_objects=[ball],
static_objects=obstacles,
robot=human_arm)
# Run inverse kinematics towards the ball
human_arm.ikine(ball.position)
# And animate it for 5 seconds
env.animate(5.0)
print "Ball hit obstacle?",
print any([obstacle.is_hit_by_sphere(ball.position, ball.radius)
for obstacle in obstacles])
print "Arm link hit obstacle?",
print any([human_arm.is_in_collision(obstacle) for obstacle in obstacles])
def get_medium_environment():
"""A medium difficulty environment for planning tests with two obstacles,
a ball as a target, and a simple human arm.
"""
obstacles = [
Obstacle([2.5, 0, 2.2], [3.5, 1, 2.5]),
Obstacle([3, 2, 1], [4, 2.5, 1.5])
]
ball = Ball([2.5, 2.5, 2.0], 0.25, target=True)
q = np.array([0, 0, 0, 0, 0, 0, 0])
robot = simple_human_arm(2.0, 2.0, q, np.array([3.0, 1.0, 0.0]))
return Environment(dynamic_objects=[ball],
static_objects=obstacles,
robot=robot)
def get_hard_environment_v2():
"""A very hard difficulty environment for planning tests with three
obstacles, a ball as a target, and a simple human arm.
"""
obstacles = [
Obstacle([2.5, 2.0, 0.0], [4.0, 2.5, 4.0]),
Obstacle([1.5, 2.0, 0.0], [2.5, 3.5, 4.0]),
Obstacle([3.2, 3.5, 0.0], [5.5, 4.0, 4.0])
]
ball = Ball([2.8, 3.8, 2.0], 0.25, target=True)
q = np.array([0, 0, 0, 0, 0, 0, 0])
robot = simple_human_arm(2.0, 2.0, q, np.array([3.0, 3.0, 0.0]))
return Environment(dynamic_objects=[ball],
static_objects=obstacles,
robot=robot)
def get_hard_environment():
"""A hard difficulty environment for planning tests with five obstacles,
a ball as a target, and a simple human arm.
"""
obstacles = [
Obstacle([0.0, 2.0, 0.0], [1.5, 2.5, 3.0]),
Obstacle([0.0, 4.0, 0.0], [1.5, 4.5, 3.0]),
Obstacle([0.0, 2.5, 0.0], [0.5, 4.0, 3.0]),
Obstacle([0.0, 2.0, 3.0], [1.5, 4.5, 3.5]),
Obstacle([0.5, 2.5, 0.0], [1.5, 4.0, 1.0])
]
ball = Ball([1.0, 3.25, 2.0], 0.5, target=True)
q = np.array([0, 0, 0, -np.pi / 2.0, 0, 0, 0])
robot = simple_human_arm(3.0, 2.0, q, np.array([1.0, 1.0, 0.0]))
return Environment(dynamic_objects=[ball],
static_objects=obstacles,
robot=robot)
def get_noodle_environment():
"""An absurd environment for our noodle arm to do planning tests. It has
five obstacles, a ball as a target, and our 10 link noodle arm
"""
obstacles = [
Obstacle([0.0, 2.0, 0.0], [1.5, 2.5, 3.0]),
Obstacle([4.0, 4.0, 0.0], [4.5, 4.5, 3.0]),
Obstacle([5.0, 0, 2.0], [5.5, 0.5, 3.0]),
Obstacle([2.0, 2.0, 3.0], [5.0, 2.5, 3.5]),
Obstacle([3.0, 4.5, 6.0], [7.0, 6.0, 5.0])
]
ball = Ball([5.0, 5.0, 3.0], 0.5, target=True)
q = np.array([0, 0, 0, 0, 0, 0, 0, 0, 0, 0])
seg_lengths = [1, 2, 1, 2, 1, 2, 1, 2, 1, 2]
robot = noodle_arm(seg_lengths, q, np.array([3.0, 1.0, 0.0]))
return Environment(dynamic_objects=[ball],
static_objects=obstacles,
robot=robot)
def get_tutorial_environment():
"""Our environment from our documentation tutorial"""
# Create an arm with a specific config and base position
q0 = np.array([0.5, 0.2, 0, 0.5, 0, 0, 0])
base_pos = np.array([2.0, 2.0, 0.0])
# And link segments of length 2.0
arm = simple_human_arm(2.0, 2.0, q0, base_pos)
# We then create a ball, target, and obstacle
ball = Ball(position=[2.0, 0.0, 2.0], radius=0.15)
target = Target(position=[5.0, 8.0, 2.0], radius=0.5)
obstacle = Obstacle([4, 4, 0], [5, 5, 2])
# And use these to create an environment with dimensions 10x10x10
return Environment(dimensions=[10, 10, 10],
dynamic_objects=[ball],
static_objects=[target, obstacle],
robot=arm)
def tutorial():
"""Code from our tutorial on the documentation"""
# Create an arm with a specific config and base position
q0 = np.array([0.5, 0.2, 0, 0.5, 0, 0, 0])
base_pos = np.array([2.0, 2.0, 0.0])
# And link segments of length 2.0
arm = simple_human_arm(2.0, 2.0, q0, base_pos)
# We then create a ball, target, and obstacle
ball = Ball(position=[2.0, 0.0, 2.0], radius=0.15)
target = Target(position=[5.0, 8.0, 2.0], radius=0.5)
obstacle = Obstacle([4, 4, 0], [5, 5, 2])
# And use these to create an environment with dimensions 10x10x10
env = Environment(dimensions=[10, 10, 10],
dynamic_objects=[ball],
static_objects=[target, obstacle],
robot=arm)
arm.ikine(ball.position)
env.animate(3.0)
arm.save_path("tutorial_path")
def plot_obstacle():
"""Create and plot an obstacle within an environment."""
obstacle = Obstacle([1.0, 1.0, 0.0], [2.0, 3.0, 5.0])
env = Environment(dimensions=[10.0, 10.0, 10.0], static_objects=[obstacle])
env.plot()