import numpy as np
from acrobotics.geometry import Shape, Collection
from acrobotics.robot import Robot, Link, Tool, DHLink
from acrobotics.util import translation, pose_z, get_default_axes3d
from acrobotics.util import plot_reference_frame

Robot models

Let’s build robots! Or at least highly simplified wireframe plots of robots. The obvious class to use is Robot. It’s a base class used to create specific robots. A robot consist of links an optionally a tool. These two objects are also classes, Link and Tool.

A robot has links

So let’s start with the most basic part, a link. We create a nice Collection of shapes to represent the link.

link_geometry = Collection([Shape(1, 0.1, 0.1)],
                           [translation(0.5, 0, 0)])

Notice that by default a box is centered around it’s reference frame. We move it by one half of it’s length in the x direction by passing the transform translation(0, 0.5, 0, 0). And we specify the Denavit-Hartenberg parameters using a named tuple DHLink with field (a, alpha, d, beta). In this case the first link of a 3R robot without offset or other strange stuff.

link_dh_parameters = DHLink(1, 0, 0, 0)

Now we can create a Link of the revolute type ('r'). For a prismatic link one uses 'p'.

first_link = Link(link_dh_parameters, 'r', link_geometry)

# general plotting administration
fig, ax = get_default_axes3d()
ax.set_title('A simple link (green) that we can rotate (blue)',
             fontsize='18')


# actually plotting the link
first_link.plot(ax, np.eye(4), c='green')

# we can even rotate the link
first_link.plot(ax, pose_z(1.0, 0, 0, 0), c='blue')

If you are confused about my seemingly arbitrary api of Collection and Shape, have a look at the page Geometry, shapes and collision checking.

A robot has tools (sometimes)

A tool is a Collection of with one extra feature, can you guess what?

Correct! A tool tip. This tell’s the robot which frame to use when calculating forward and inverse kinematics. For example, a welding robot has a tip of the welding torch that has to be at the right place in the right time. This is where the tip frame is located. I have modelled a nice torch using boxes to show you this:

from acrobotics.recources.torch_model import torch

fig, ax = get_default_axes3d(xlim=[-0.2, 0.2], ylim=[-0.2, 0.2], zlim=[-0.2, 0.2])
ax.set_title('A welding torch with a tip reference frame', fontsize='18')
torch.plot(ax, c='black')
plot_reference_frame(ax, torch.tf_tt)

A robot has robots

Uh, wait?! Ok let’s bring it all togheter and create the 3R robot a mentioned earlier. But let’s call it PlanarArm, as Siciliano does in his book.

class PlanarArm(Robot):
    """ Robot defined on page 69 in book Siciliano """

    def __init__(self, a1=1, a2=1, a3=1):
        geometry = [Collection([Shape(a1, 0.1, 0.1)], [translation(-a1/2, 0, 0)]),
                    Collection([Shape(a2, 0.1, 0.1)], [translation(-a2/2, 0, 0)]),
                    Collection([Shape(a3, 0.1, 0.1)], [translation(-a3/2, 0, 0)])]
        super().__init__(
                [Link(DHLink(a1, 0, 0, 0), 'r', geometry[0]),
                 Link(DHLink(a2, 0, 0, 0), 'r', geometry[1]),
                 Link(DHLink(a3, 0, 0, 0), 'r', geometry[2])]
                )

robot = PlanarArm(a3 = 0.2)

By default you can calculate foward kinematics using robot.fk(joint_values), check for collision is_in_collision(Collection) and visualize the robot (discussed in the next section).

robot.fk([0, 0, 0])

array([[1. , 0. , 0. , 2.2],
       [0. , 1. , 0. , 0. ],
       [0. , 0. , 1. , 0. ],
       [0. , 0. , 0. , 1. ]])

joint_angles = [0.6, -1.1, 0.3]

good_box = Collection([Shape(0.5, 0.5, 0.5)], [translation(1.5, 1.5, 0)])
bad_box = Collection([Shape(0.5, 0.5, 0.5)], [translation(1.5, 0, 0)])

res1 = robot.is_in_collision(joint_angles, good_box)
res2 = robot.is_in_collision(joint_angles, bad_box)

print('Collides with good box? {}'.format(res1))
print('Collides with bad box? {}'.format(res2))

Collides with good box? False
Collides with bad box? True

fig, ax = get_default_axes3d(xlim=[0, 2], ylim=[0, 2], zlim=[-1, 1])
ax.set_title('Collision objects', fontsize='18')
ax.view_init(elev=60)

robot.plot(ax, joint_angles, c='black')
good_box.plot(ax, c='green')
bad_box.plot(ax, c='red')

But inverse kinematics don’t come for free. Analytic inverse kinematics are implemented for specific robots in acrobotics.resources.robots.

For example the anthropomorphic arm is a robot with 3 revolute joints. The frist one parellel with the z-axis and the other two perpendicular to the first. (Kind of the famous 2R arm that can move in 3D like a crane.) For a reachable position in Cartesian space, there are multiple robot configurations that get the end-effector in this position. (The orientation is ignored bij the inverse kinematics algorithm.)

from acrobotics.recources.robots import AnthropomorphicArm

robot_2 = AnthropomorphicArm(a3=0.7)
pose = translation(1.0, 0.2, 0.5)

ik_sol = robot_2.ik(pose)
print('Can we reach this position? {}'.format(ik_sol['success']))

fig, ax = get_default_axes3d(xlim=[0, 1], ylim=[-0.5, 0.5], zlim=[0, 1])
#ax.set_axis_off()
colors = ['navy', 'orange']

for color, qi in zip(colors, ik_sol['sol']):
    robot_2.plot(ax, qi, c=color)

plot_reference_frame(ax, pose)

Can we reach this position? True

Show me them robots

Now we can visualize the robot using the Robot.plot(axes_handle, joint_values, ...) function. This is a nice api, apart from all the setup code to create a nice plot. Using the get_default_axes3d from acrobotics.util greatly reduces this setup code (by 8 lines).

# specify some joint angles for the plot
q_plot = [0.5, 0.3, -0.5]

fig, ax = get_default_axes3d(xlim=[0, 2], ylim=[0, 2], zlim=[-1, 1])
ax.set_title('A 3R planar arm', fontsize='18')
ax.view_init(elev=60)

robot.plot(ax, q_plot, c='black')

Notice that the link frames lie at the end of a link. This is a confusing … from using Denavit-Hartenberg parameters. If I get to annoyed by it I will change this.

fig, ax = get_default_axes3d(xlim=[0, 2], ylim=[0, 2], zlim=[-1, 1])
ax.set_title('Link reference frames', fontsize='18')
ax.view_init(elev=60)

robot.plot(ax, q_plot, c='black')

tf_links = robot.fk_all_links(q_plot)
for tfl in tf_links: plot_reference_frame(ax, tfl, 0.7)

Also you can just plot the kinematics structure of the robot to get an idea whether you did it right without all the wireframes getting in your way.

fig, ax = get_default_axes3d(xlim=[0, 2], ylim=[0, 2], zlim=[-1, 1])
ax.set_title('Robot X-ray', fontsize='18')
ax.view_init(elev=60)

robot.plot_kinematics(ax, q_plot)

Give it some tools

We can give the robot a pen to write on a blackboard. (A long stick as end-effector and a flat plane as collision environment.)

# create with a list of shapes, a list of tfs for the shapes
# and a tf to specify the tool tip relative to the last link
pen = Tool([Shape(0.3, 0.05, 0.05)],
           [translation(0.15, 0, 0)],
           translation(0.3, 0, 0))

robot.tool = pen

blackboard = Collection([Shape(0.1, 1.0, 0.5)],
                        [translation(2, 0, 0.25)])

fig, ax = get_default_axes3d(xlim=[0, 2],
                             ylim=[0, 2],
                             zlim=[-1, 1])
ax.view_init(elev=70, azim = -90)

q1 = [0.5, 0.3, -0.5]
q2 = [0.6, -1.2, 0.6]

robot.plot(ax, q1, c='navy')
robot.plot(ax, q2, c='purple')

plot_reference_frame(ax, robot.fk(q1))
blackboard.plot(ax, c='black')

The tool is also added during collision checking with the environment.

print(robot.is_in_collision(q1, blackboard))
print(robot.is_in_collision(q2, blackboard))

False
True