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 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 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 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")