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 plot_arm():
"""Shows how to plot an arm."""
# Create an arm at a certain position and joint angle
q0 = np.array([0, 0, 0, -2.0, 0, 0, 0])
base_pos = np.array([1.0, 1.0, 0.0])
human_arm = simple_human_arm(2.0, 1.0, q0, base_pos)
env = Environment(dimensions=[3.0, 3.0, 3.0], robot=[human_arm])
env.plot()
def find_jacob0():
"""Show how to calculate the jacobian of am arm."""
np.set_printoptions(suppress=True)
# Create arm
q0 = np.array([0, -np.pi / 4, 0, -np.pi / 4, 0, 0, 0])
arm = simple_human_arm(2, 1, q0)
# Calculate jacobian on that joint config
print arm.jacob0(q0)
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_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 arm_animation():
"""Animate the arm moving to touch a ball"""
# Declare a human arm
q0 = np.array([0.5, 0.2, 0, 0.5, 1.5])
human_arm = simple_human_arm(2.0, 2.0, q0, np.array([2.0, 2.0, 0.0]))
# Create a ball as our target
ball = Ball(np.array([3.0, 2.0, 3.0]), 0.15, target=True)
# Create our environment
env = Environment([5.0, 5.0, 5.0], dynamic_objects=[ball],
robot=human_arm)
# Run inverse kinematics to find a joint config that lets arm touch ball
human_arm.ikine(ball.position)
# Animate
env.animate(5.0)
def arm_ball_animation():
"""Animate an arm holding a ball, moving around to an arbitrary spot."""
# Create a human arm
q0 = np.array([0.5, 0.2, 0, 0.5, 1.5])
human_arm = simple_human_arm(2.0, 2.0, q0, np.array([2.0, 2.0, 0.0]))
# Create a ball the arm can hold
ball = Ball(np.array([0.0, 0.0, 0.0]), 0.15)
human_arm.hold(ball)
# Declare our environment
env = Environment([5.0, 5.0, 5.0], dynamic_objects=[ball],
robot=human_arm)
# Find joint config to random location show we can show arm moving
# while holding the ball
position = human_arm.ikine([3.0, 2.0, 3.0])
# Animate for 3 seconds
env.animate(3.0)
def plot():
"""Generic plotting example given an environment with some
objects in it and a robot.
"""
# Create our arm
q0 = np.array([0.5, 0.2, 0, 0.5, 0, 0, 0])
human_arm = simple_human_arm(2.0, 2.0, q0, np.array([2.0, 2.0, 0.0]))
# And our objects
ball = Ball(np.array([2.0, 0.0, 2.0]), 0.15)
target = Target(np.array([5.0, 8.0, 2.0]), 0.5)
env = Environment(dimensions=[10.0, 10.0, 10.0],
dynamic_objects=[ball],
static_objects=[target],
robot=human_arm)
# Make the arm touch the ball
new_q = human_arm.ikine(ball.position, 10000, 0.01)
human_arm.update_angles(new_q)
# Plot it
env.plot()
def __init__(self, num_joints=4):
self.actions = [-1, 1] * num_joints
self.actions += ["Throw"]
self.move_count = 0
self.collected_rewards = []
self.released = False
self.num_actions = len(self.actions)
self.observation_size = len(self.actions)
self.ball = Ball(np.array([0, 0, 0]), 0.15)
self.target = Target(np.array([2.0, 10.0, 2.0]), 0.5)
q = np.array([0, 0, 0, np.pi / 2, 0, 0, 0])
self.robot = simple_human_arm(1.0, 1.0, q, np.array([2.0, 2.0, 2.0]))
room_dimensions = np.array([10.0, 10.0, 20.0])
self.env = Environment(room_dimensions,
dynamic_objects=[self.ball],
static_objects=[self.target],
robot=self.robot)
self.max_distance = np.linalg.norm(room_dimensions)
self.distance_to_target = self.max_distance
