def ball_animation():
"""Create a few balls and have them all move around the environment
at different velocities.
"""
# Create 5 balls moving at random velocities
balls = []
for _ in xrange(5):
# Create a ball at a random starting position
starting_pos = np.random.rand(3) * 10
ball = Ball(starting_pos, radius=0.15)
# And generate a random velocity to "throw" it at
velocity = np.random.uniform(-10, 10, 3)
ball.throw(velocity)
# Add it on the list we will feed to the environment
balls.append(ball)
# Add a target, just to show one
target = Target(np.array([5.0, 18.0, 2.0]), 0.5)
# Declare an environment with our room, balls, and target
env = Environment(dimensions=[10.0, 20.0, 100.0],
dynamic_objects=balls,
static_objects=[target])
# Animate it for 5 seconds.
env.animate(5.0)
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])
# q0 = np.array([0, 0.4, -0.4, -1.5, 0])
q0 = np.array([-1.5, 1.9, 1.1, 1.1, 1.5])
base_pos = np.array([2.0, 2.0, 0.0])
seg_lens = np.array([2.0, 4.0, 4.0, 2.0])
# And link segments of length 2.0
# arm = simple_human_arm(2.0, 2.0, q0, base_pos)
arm = inchworm(seg_lens, q0, base_pos)
# We then create a ball, target, and obstacle
ball = Ball(position=[1.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],
robot=[arm])
# arm.ikine(target.position)
# env.animate(3.0)
# arm.save_path("tutorial_path")
env.plot()
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 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])
# arm = simple_human_arm(2.0, 2.0, q0, np.array([2.0, 2.0, 0.0]))
q0 = np.array([0.7, 1.9, 1.1, 0])
qTest = np.array([0, 0, 0, 0])
base_pos = np.array([0., 0., 0.])
seg_lens = np.array([2.0, 4.0, 4.0, 2.0])
arm = inchworm(seg_lens, qTest, base_pos)
# q0_2 = np.array([0.7, 1.9, 1.1, 0, 1.5])
# base_pos_2 = np.array([10., 10., 0.])
# seg_lens_2 = np.array([2.0, 4.0, 4.0, 2.0])
#
# arm2 = inchworm(seg_lens_2, q0_2, base_pos_2)
# Create a ball as our target
ball = Ball(np.array([4, 0., 0.]), 0.15, target=True)
ball_2 = Ball(np.array([6, 0., 0.]), 0.15, target=True)
ball_3 = Ball(np.array([7, 1., 0.]), 0.15, target=True)
ball_4 = Ball(np.array([5, 5., 0.]), 0.15, target=True)
# Create our environment
env = Environment([20.0, 20.0, 5.0],
dynamic_objects=[ball, ball_2, ball_3, ball_4],
robot=[arm])
ser = serial.Serial(port='COM9',
baudrate=9600,
parity=serial.PARITY_NONE,
stopbits=serial.STOPBITS_ONE,
bytesize=serial.EIGHTBITS,
timeout=3.0)
time.sleep(1)
# Run inverse kinematics to find a joint config that lets arm touch ball
# arm.ikine(ball.position)
# arm2.ikine(ball_2.position)
# arm.ikineConstrained(ball.position)
q = arm.ikineConstrained(ball.position, ser)
# Animate
env.animate(5.0, 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 __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
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 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 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 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 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)
class ThrowingArm(object):
actions = []
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
def observe(self):
"""Returns current observation"""
landing_positions = []
for i, action in enumerate(self.actions):
if action == "Throw":
landing_pos = self.env.hypothetical_landing_position()
else:
link = i / 2
# Perform link update
self.robot.update_link_velocity(link, ACCEL_CHANGE *\
self.actions[i], TIME)
# Calculate hypothetical landing spot as observation
landing_pos = self.env.hypothetical_landing_position()
# Undo link update
self.robot.update_link_velocity(link, -ACCEL_CHANGE *\
self.actions[i], TIME)
distance = np.linalg.norm(landing_pos - self.target.position)
landing_positions.append(distance)
return np.array(landing_positions)
def perform_action(self, action):
"""Update internal state to reflect the fact that an action was taken
:param action: Number of the action performed
"""
if action < 8:
# Update a specific links velocity
link = action / 2
self.robot.update_link_velocity(link, ACCEL_CHANGE *\
self.actions[action], TIME)
self.move_count += 1
else:
# Or release the ball
self.robot.release()
self.released = True
def step(self, dt):
"""Update internal state as if time dt has passed"""
pass
def is_over(self):
"""Check if simulation is over"""
return self.released
def collect_reward(self, action):
"""Returns reward accumulated since last time this
function was called.
"""
# Get previous landing position
prior_landing_pos = self.env.hypothetical_landing_position()
prior_dist = np.linalg.norm(self.target.position - prior_landing_pos)
# Then perform the action and get new landing position
self.perform_action(action)
new_landing_pos = self.env.hypothetical_landing_position()
new_dist = np.linalg.norm(self.target.position - new_landing_pos)
if self.actions[action] == "Throw":
# If we threw it, see the distance
reward = -new_dist + 1.0
else:
# Otherwise, see how much we improved
reward = (prior_dist - new_dist)
self.distance_to_target = new_dist
self.collected_rewards.append(reward)
return reward
def display_actions(self):
print "Moved {} times before throwing!".format(self.move_count)
print "Last reward: {}".format(self.collected_rewards[-1])
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()