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