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)