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")
UR5¶
In [72]:
UR5 = rsb.models.DH.UR5()
fig = UR5.plotly(q=UR5.qz)
fig.show("png")
GenericFour¶
In [31]:
G4_planar = rsb.models.DH.Generic.GenericFour()
fig = G4_planar.plotly(q=G4_planar.qz)
fig.show("png")
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")
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)
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")
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")