def test_multiple_robots():
p = SE3()
p1 = p
p2 = p.Tx(1)
p3 = p.Tx(2)
robot = gph.GraphicalRobot(g_canvas, 'Robot A')
robot.append_link('r', p1, 1.0)
robot.append_link('R', p2, 1.0)
robot.append_link('r', p3, 1.0)
arr = array([[0, 0, 1, 0], [0, 1, 0, 0], [1, 0, 0, 0], [0, 0, 0, 1]])
new_p1 = SE3(arr)
arr = array([[0, 0, 1, 0], [0, 1, 0, 0], [1, 0, 0, 1], [0, 0, 0, 1]])
new_p2 = SE3(arr)
arr = array([[0, 0, 1, 0], [0, 1, 0, 0], [1, 0, 0, 2], [0, 0, 0, 1]])
new_p3 = SE3(arr)
robot2 = gph.GraphicalRobot(g_canvas, 'Robot B')
robot2.append_link('r', new_p1, 1.0)
robot2.append_link('R', new_p2, 1.0)
robot2.append_link('r', new_p3, 1.0)
def plot(self, jointconfig, unit='rad'):
"""
Creates a 3D plot of the robot in your web browser
:param jointconfig: takes an array or list of joint angles
:param unit: unit of angles. radians if not defined
:return: a vpython robot object.
"""
if type(jointconfig) == list:
jointconfig = argcheck.getvector(jointconfig)
if unit == 'deg':
jointconfig = jointconfig * pi / 180
if jointconfig.size == self.length:
poses = self.fkine(jointconfig, unit, alltout=True)
if self.roplot is None:
# No current plot, create robot plot
self.g_canvas = gph.GraphicsCanvas3D()
print("canvas created")
self.roplot = gph.GraphicalRobot(self.g_canvas, self.name, self)
# Move existing plot
self.roplot.set_joint_poses(poses)
return
def test_graphical_robot_creation():
"""
This test will create a simple 3-link graphical robot.
The joints are then set to new poses to show updating does occur.
"""
p = SE3()
p1 = p
p2 = p.Tx(1)
p3 = p.Tx(2)
robot = gph.GraphicalRobot(g_canvas, 'test_3_link_robot')
robot.append_link('r', p1, 1.0)
robot.append_link('R', p2, 1.0)
robot.append_link('r', p3, 1.0)
arr = array([[0, 0, 1, 0], [0, 1, 0, 0], [1, 0, 0, 0], [0, 0, 0, 1]])
new_p1 = SE3(arr)
arr = array([[0, 0, 1, 0], [0, 1, 0, 0], [1, 0, 0, 1], [0, 0, 0, 1]])
new_p2 = SE3(arr)
arr = array([[0, 0, 1, 0], [0, 1, 0, 0], [1, 0, 0, 2], [0, 0, 0, 1]])
new_p3 = SE3(arr)
sleep(2)
robot.set_joint_poses([new_p1, new_p2, new_p3])
def test_seriallink():
L = []
L2 = []
L.append(Link(a=1, jointtype='R'))
L.append(Link(a=1, jointtype='R'))
L.append(Link(a=1, jointtype='R'))
L2.append(Link(a=1, jointtype='R'))
L2.append(Link(a=1, jointtype='R'))
L2.append(Link(a=1, jointtype='R'))
tl = SerialLink(L, name='R1')
tl2 = SerialLink(L, name='R2')
robot = gph.GraphicalRobot(g_canvas, '', seriallink=tl)
robot2 = gph.GraphicalRobot(g_canvas, '', seriallink=tl2)
def test_multiple_canvases():
p = SE3()
p1 = p
p2 = p.Tx(1)
p3 = p.Tx(2)
robot = gph.GraphicalRobot(g_canvas, 'Robot A1')
robot.append_link('r', p1, 1.0)
robot.append_link('R', p2, 1.0)
robot.append_link('r', p3, 1.0)
sleep(5)
g_canvas2 = gph.GraphicsCanvas(title='Scene B')
robot2 = gph.GraphicalRobot(g_canvas2, 'Robot B1')
robot2.append_link('r', SE3().Rand(), 1.0)
robot2.append_link('R', SE3().Rand(), 1.0)
robot2.append_link('r', SE3().Rand(), 1.0)
def plot(self, jointconfig, unit='rad'):
"""
Creates a 3D plot of the robot in your web browser
:param jointconfig: takes an array or list of joint angles
:param unit: unit of angles. radians if not defined
:return: a vpython robot object.
"""
if type(jointconfig) == list:
jointconfig = argcheck.getvector(jointconfig)
if unit == 'deg':
jointconfig = jointconfig * pi / 180
if jointconfig.size == self.length:
poses = self.fkine(jointconfig, unit, alltout=True)
if self.roplot is None:
# No current plot, create robot plot
self.g_canvas = gph.GraphicsCanvas3D()
print("canvas created")
self.roplot = gph.GraphicalRobot(self.g_canvas, self.name)
for i in range(1, len(poses)):
# calculate length of joint
x1 = poses[i - 1].t[0]
x2 = poses[i].t[0]
y1 = poses[i - 1].t[1]
y2 = poses[i].t[1]
z1 = poses[i - 1].t[2]
z2 = poses[i].t[2]
length = sqrt((x2 - x1) * (x2 - x1) + (y2 - y1) * (y2 - y1) +
(z2 - z1) * (z2 - z1))
# add joint to robot
self.roplot.append_link(
self.links[i - 1].jointtype,
poses[i],
length,
)
return
else:
# Move existing plot
self.roplot.set_joint_poses(poses[1:4])
return
def test_animate_joints():
"""
This test will create a three link robot, and iterate through a series of frames to animate movement.
"""
p = SE3()
p1 = p
p2 = p.Tx(1)
p3 = p.Tx(2)
robot = gph.GraphicalRobot(g_canvas, 'Robot Animation')
robot.append_link('r', p1, 1.0)
robot.append_link('R', p2, 1.0)
robot.append_link('r', p3, 1.0)
robot.animate(
[[SE3().Rand(), SE3().Rand(), SE3().Rand()],
[SE3().Rand(), SE3().Rand(), SE3().Rand()],
[SE3().Rand(), SE3().Rand(), SE3().Rand()],
[SE3().Rand(), SE3().Rand(), SE3().Rand()],
[SE3().Rand(), SE3().Rand(), SE3().Rand()],
[SE3().Rand(), SE3().Rand(), SE3().Rand()],
[SE3().Rand(), SE3().Rand(), SE3().Rand()]], 7)
import graphics as gph
from roboticstoolbox.robot.serial_link import SerialLink, Link
if __name__ == "__main__":
canvas = gph.GraphicsCanvas3D()
L = []
L2 = []
L.append(Link(a=1, jointtype='R'))
L.append(Link(a=1, jointtype='R'))
L.append(Link(a=1, jointtype='R'))
L2.append(Link(a=1, jointtype='R'))
L2.append(Link(a=1, jointtype='R'))
L2.append(Link(a=1, jointtype='R'))
tl = SerialLink(L, name='R1')
tl2 = SerialLink(L, name='R2')
robot = gph.GraphicalRobot(canvas, '', seriallink=tl)
robot2 = gph.GraphicalRobot(canvas, '', seriallink=tl2)
