In [5]:
import robosandbox as rsb
import numpy as np

Plot Robots¶

plot robot models figures used in paper

Panda¶

In [23]:
panda = rsb.models.DH.Panda()
fig = panda.plotly(q=panda.qr)
fig.show("png")
No description has been provided for this image

UR5¶

In [72]:
UR5 = rsb.models.DH.UR5()
fig = UR5.plotly(q=UR5.qz)
fig.show("png")
No description has been provided for this image

GenericFour¶

In [31]:
G4_planar = rsb.models.DH.Generic.GenericFour()
fig = G4_planar.plotly(q=G4_planar.qz)
fig.show("png")
No description has been provided for this image
In [71]:
G4_modified = rsb.models.DH.Generic.GenericFour(
    alpha=[0, np.pi / 2, 0, 0],
)
fig = G4_modified.plotly(q=G4_modified.qz)
fig.show("png")
No description has been provided for this image
In [99]:
import robosandbox.models.DH.Generic as generic
from spatialmath import SE3

c1 = generic.Generic(
    dofs=4,
    a=[0, -0.4, -0.4, -0.4],
    d=[0.4, 0, 0, 0],
    alpha=[np.pi / 2, 0, 0, 0],
    qlim=[[-np.pi, np.pi]] * 4,
    offset=[0, -np.pi / 2, 0, 0],
)
c1.base = SE3(0.2, 0, 0)  # move 0.2 m along x axis
fig = c1.plotly(q=c1.qz, zoom_factor=0.1)

c2 = generic.Generic(
    dofs=4,
    a=[0, -0.4, -0.4, -0.4],
    d=[0.4, 0, 0, 0],
    alpha=[np.pi / 2, np.pi / 2, 0, 0],
    qlim=[[-np.pi, np.pi]] * 4,
    offset=[0, -np.pi / 2, 0, 0],
)
c2.base = SE3(-0.2, 0, 0)  # move 0.2 m along x axis
fig = c2.plotly(q=c2.qz, zoom_factor=0.1, fig=fig)
fig.update_layout(
    scene=dict(
        camera=dict(
            eye=dict(x=0.5, y=-0.5, z=0.5),
            center=dict(x=0, y=0, z=0.25),
        ),
    )
)
fig.update_layout(showlegend=False)
fig.show("png")

fig.write_image("fig/Generic4.png", scale=5)
No description has been provided for this image
In [75]:
import os
import swift

pwd = os.getcwd()
tld = os.path.join(pwd, "data", "SO101")

SO101 = rsb.models.URDF.Model(
    name="SO101",
    file_path="so101_new_calib.urdf",
    tld=tld,
)
SO101.q = np.zeros(6)
print(SO101)

env = swift.Swift()
env.launch(realtime=True, browser="notebook")
env.add(SO101)
ERobot: so101_new_calib, 6 joints (RRRRRR), dynamics, geometry, collision
┌──────┬───────────┬───────┬───────────┬───────────────────────────────────────────────────────────────────────┐
│ link │   link    │ joint │  parent   │                          ETS: parent to link                          │
├──────┼───────────┼───────┼───────────┼───────────────────────────────────────────────────────────────────────┤
│    0 │ base      │       │ BASE      │ SE3()                                                                 │
│    1 │ shoulder  │     0 │ base      │ SE3(0.02079, -0.02307, 0.09488; -180°, 3.459e-14°, 90°) ⊕ Rz(q0)      │
│    2 │ upper_arm │     1 │ shoulder  │ SE3(-0.0304, -0.01828, -0.0542; 90°, -90°, 180°) ⊕ Rz(q1)             │
│    3 │ lower_arm │     2 │ upper_arm │ SE3(-0.1126, -0.028, 2.463e-16; -7.037e-14°, 3.3e-14°, 90°) ⊕ Rz(q2)  │
│    4 │ wrist     │     3 │ lower_arm │ SE3(-0.1349, 0.0052, 1.652e-16; 1.861e-13°, 1.64e-13°, -90°) ⊕ Rz(q3) │
│    5 │ gripper   │     4 │ wrist     │ SE3(0, -0.0611, 0.0181; 90°, -5.375e-06°, 180°) ⊕ Rz(q4)              │
│    6 │ @jaw      │     5 │ gripper   │ SE3(0.0202, 0.0188, -0.0234; 90°, -2.946e-15°, -7.944e-13°) ⊕ Rz(q5)  │
└──────┴───────────┴───────┴───────────┴───────────────────────────────────────────────────────────────────────┘

Out[75]:
0

Optimized UR5 - P1¶

In [8]:
from robosandbox.models.DH.Generic import Generic


def create_p1():
    """Create and return a 6-DOF robot."""
    design_params = np.load("./data/optimze_for_ranges.npy")
    d0, a1, a2, d3, d4, d5 = design_params
    d = [d0, 0, 0, d3, d4, d5]  # d1, d2 are 0
    a = [0, a1, a2, 0, 0, 0]  # a0, a3, a4, a5 are 0
    alpha = [np.pi / 2, 0, 0, np.pi / 2, -np.pi / 2, 0]  # from UR5
    return Generic(dofs=6, a=a, alpha=alpha, d=d)


P1 = create_p1()
P1.plotly(q=P1.qz).show("png")
No description has been provided for this image
In [ ]:
def create_p2():
    """Create and return a 6-DOF robot."""
    design_params = np.load("./data/optimize_for_G_ranges.npy")
    d0, alpha0, a1, a2, d3, alpha3, d4, alpha4, d5 = design_params
    d = [d0, 0, 0, d3, d4, d5]
    a = [0, a1, a2, 0, 0, 0]
    alpha = [alpha0, 0, 0, alpha3, alpha4, 0]
    return Generic(dofs=6, a=a, alpha=alpha, d=d)


P2 = create_p2()
P2.plotly(q=P2.qz).show("png")
No description has been provided for this image