def test_collided(self):
s0 = rtb.Box([1, 1, 1], sm.SE3(0, 0, 0))
s1 = rtb.Box([1, 1, 1], sm.SE3(3, 0, 0))
p = rtb.models.Panda()
c0 = p.collided(s0)
c1 = p.collided(s1)
self.assertTrue(c0)
self.assertFalse(c1)
def test_collided(self):
s0 = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
s1 = rp.Box([1, 1, 1], sm.SE3(3, 0, 0))
p = rp.models.Panda()
link = p.links[3]
c0 = link.collided(s0)
c1 = link.collided(s1)
self.assertTrue(c0)
self.assertFalse(c1)
def test_collision(self):
s0 = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
s1 = rp.Box([1, 1, 1], sm.SE3(0.5, 0, 0))
s2 = rp.Box([1, 1, 1], sm.SE3(3, 0, 0))
c0 = s0.collided(s1)
c1 = s0.collided(s2)
self.assertTrue(c0)
self.assertFalse(c1)
def test_dist(self):
s0 = rtb.Box([1, 1, 1], sm.SE3(0, 0, 0))
s1 = rtb.Box([1, 1, 1], sm.SE3(3, 0, 0))
p = rtb.models.Panda()
d0, _, _ = p.closest_point(s0)
d1, _, _ = p.closest_point(s1, 5)
d2, _, _ = p.closest_point(s1)
self.assertAlmostEqual(d0, -0.5599999999995913)
self.assertAlmostEqual(d1, 2.4275999999999995)
self.assertAlmostEqual(d2, None)
def test_dist(self):
s0 = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
s1 = rp.Box([1, 1, 1], sm.SE3(3, 0, 0))
p = rp.models.Panda()
link = p.links[3]
d0, _, _ = link.closest_point(s0)
d1, _, _ = link.closest_point(s1, 5)
d2, _, _ = link.closest_point(s1)
self.assertAlmostEqual(d0, -0.49)
self.assertAlmostEqual(d1, 2.44)
self.assertAlmostEqual(d2, None)
def test_color(self):
shape = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
shape.color = [1, 2, 3]
self.assertEquals(shape.color[0], 1)
self.assertEquals(shape.color[1], 2)
self.assertEquals(shape.color[2], 3)
self.assertEquals(shape.color[3], 1)
shape.color = [1, 2, 3, 0.5]
self.assertEquals(shape.color[0], 1)
self.assertEquals(shape.color[1], 2)
self.assertEquals(shape.color[2], 3)
self.assertEquals(shape.color[3], 0.5)
shape.color = (1, 2, 3)
self.assertEquals(shape.color[0], 1)
self.assertEquals(shape.color[1], 2)
self.assertEquals(shape.color[2], 3)
self.assertEquals(shape.color[3], 1)
shape.color = (1, 2, 3, 0.5)
self.assertEquals(shape.color[0], 1)
self.assertEquals(shape.color[1], 2)
self.assertEquals(shape.color[2], 3)
self.assertEquals(shape.color[3], 0.5)
def test_color(self):
shape = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
shape.color = [0.1, 0.2, 0.3]
self.assertEqual(shape.color[0], 0.1)
self.assertEqual(shape.color[1], 0.2)
self.assertEqual(shape.color[2], 0.3)
self.assertEqual(shape.color[3], 1)
shape.color = [0.1, 0.2, 0.3, 0.5]
self.assertEqual(shape.color[0], 0.1)
self.assertEqual(shape.color[1], 0.2)
self.assertEqual(shape.color[2], 0.3)
self.assertEqual(shape.color[3], 0.5)
shape.color = (0.1, 0.2, 0.3)
self.assertEqual(shape.color[0], 0.1)
self.assertEqual(shape.color[1], 0.2)
self.assertEqual(shape.color[2], 0.3)
self.assertEqual(shape.color[3], 1)
shape.color = (100, 200, 250, 100)
self.assertAlmostEqual(shape.color[0], 100 / 255)
self.assertAlmostEqual(shape.color[1], 200 / 255)
self.assertAlmostEqual(shape.color[2], 250 / 255)
self.assertEqual(shape.color[3], 100 / 255)
def test_dict(self):
panda = rtb.models.Panda()
panda.grippers[0].links[0].collision.append(rtb.Box([1, 1, 1]))
panda.to_dict()
wx = rtb.models.wx250s()
wx.to_dict()
def test_collision_fail(self):
l0 = rp.ELink()
col = rp.Box([1, 1, 1])
l0.collision = col
with self.assertRaises(TypeError):
l0.collision = [1, 1, 1]
with self.assertRaises(TypeError):
l0.collision = 1
def test_geometry_fail(self):
l0 = rp.ELink()
col = rp.Box([1, 1, 1])
l0.geometry = col
l0.geometry = [col, col]
with self.assertRaises(TypeError):
l0.geometry = [1, 1, 1]
with self.assertRaises(TypeError):
l0.geometry = 1
def test_closest(self):
s0 = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
s1 = rp.Cylinder(1, 1, sm.SE3(2, 0, 0))
s2 = rp.Sphere(1, sm.SE3(4, 0, 0))
d0, _, _ = s0.closest_point(s1, 10)
d1, _, _ = s1.closest_point(s2, 10)
d2, _, _ = s2.closest_point(s0, 10)
d3, _, _ = s2.closest_point(s0)
self.assertAlmostEqual(d0, 0.5)
self.assertAlmostEqual(d1, 4.698463840213662e-13)
self.assertAlmostEqual(d2, 2.5)
self.assertAlmostEqual(d3, None)
def test_fkdict(self):
panda = rtb.models.Panda()
panda.grippers[0].links[0].collision.append(rtb.Box([1, 1, 1]))
panda.fk_dict()
# Configurations q0 = [-0.5653, -0.1941, -1.2602, -0.7896, -2.3227, -0.3919, -2.5173] q1 = [-0.1361, -0.1915, -1.2602, -0.8652, -2.8852, -0.7962, -2.039] q2 = [0.2341, -0.2138, -1.2602, -0.4709, -3.0149, -0.7505, -2.0164] q3 = [0.1584, 0.3429, -1.2382, -0.9829, -2.0892, -1.6126, -0.5582] q4 = [0.3927, 0.1763, -1.2382, -0.1849, -1.96, -1.4092, -1.0492] q5 = [-0.632, 0.5012, -1.2382, -0.8353, 2.2571, -0.1041, 0.3066] q6 = [0.1683, 0.7154, -0.4195, -1.0496, 2.4832, -0.6028, -0.6401] q7 = [-0.1198, 0.5299, -0.6291, -0.4348, 2.1715, -1.6403, 1.8299] q8 = [0.2743, 0.4088, -0.5291, -0.4304, 2.119, -1.9994, 1.7162] q9 = [0.2743, 0.4088, -0.5291, -0.4304, -0.9985, -1.0032, -1.7278] qs = [q0, q1, q2, q3, q4, q5, q6, q7, q8, q9] # Collisions s1 = rp.Box(scale=[0.60, 1.1, 0.02], base=sm.SE3(0.95, 0, 0.20)) s2 = rp.Box(scale=[0.60, 1.1, 0.02], base=sm.SE3(0.95, 0, 0.60)) s3 = rp.Box(scale=[0.60, 1.1, 0.02], base=sm.SE3(0.95, 0, 1.00)) s4 = rp.Box(scale=[0.60, 1.1, 0.02], base=sm.SE3(0.95, 0, 1.40)) s5 = rp.Box(scale=[0.60, 0.02, 1.40], base=sm.SE3(0.95, 0.55, 0.7)) s6 = rp.Box(scale=[0.60, 0.02, 1.40], base=sm.SE3(0.95, -0.55, 0.7)) s = [s1, s2, s3, s4, s5, s6] # Launch Simulator env = rp.backend.Swift()
def test_init(self):
rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
rp.Cylinder(1, 1, sm.SE3(2, 0, 0))
rp.Sphere(1, sm.SE3(4, 0, 0))
def test_wt(self):
s0 = rp.Box([1, 1, 1], sm.SE3(0, 0, 0))
s0.wT = np.eye(4)
# @author Jesse Haviland
# """
import roboticstoolbox as rtb
import spatialmath as sm
import numpy as np
import pathlib
import os
path = os.path.realpath('.')
# Launch the simulator Swift
env = rtb.backends.Swift()
env.launch()
b1 = rtb.Box(scale=[0.60, 1.1, 0.02], base=sm.SE3(1.95, 0, 0.20))
b2 = rtb.Box(scale=[0.60, 1.1, 0.02], base=sm.SE3(1.95, 0, 0.60))
b3 = rtb.Box(scale=[0.60, 1.1, 0.02], base=sm.SE3(1.95, 0, 1.00))
b4 = rtb.Box(scale=[0.60, 1.1, 0.02], base=sm.SE3(1.95, 0, 1.40))
b5 = rtb.Box(scale=[0.60, 0.02, 1.40], base=sm.SE3(1.95, 0.55, 0.7))
b6 = rtb.Box(scale=[0.60, 0.02, 1.40], base=sm.SE3(1.95, -0.55, 0.7))
collisions = [b1, b2, b3, b4, b5, b6]
path = pathlib.Path(path) / 'roboticstoolbox' / 'data'
import roboticstoolbox as rp
import spatialmath as sm
import numpy as np
import time
# Configurations
q0 = [-0.3728, 1.0259, -0.9876, -1.2082, 2.0677, -0.563, -1.6563]
q1 = [0.2993, 0.1747, -1.7835, -0.8593, 3.1042, -1.2145, -1.3626]
q2 = [0.3537, 0.2079, -1.7168, -0.2937, 3.0224, -0.7083, -1.3286]
q3 = [0.3398, -0.2349, -0.0415, -1.5042, 2.7647, -1.7995, 3.0539]
qs = [q0, q1, q2, q3]
# Collisions
s1 = rp.Box(
scale=[0.6, 0.04, 0.6],
base=sm.SE3(0.65, -0.15, 0.43))
s2 = rp.Box(
scale=[0.6, 0.04, 0.6],
base=sm.SE3(0.65, -0.15, 1.22))
s3 = rp.Box(
scale=[0.3, 0.04, 0.2],
base=sm.SE3(0.8, -0.15, 0.83))
s = [s1, s2, s3]
# Launch Simulator
env = rp.backend.Swift()
env.launch()
q0 = [0.4522, -0.0688, 0.4516, -1.3564, 2.6366, -1.2885, 2.7515] q1 = [0.5645, -0.2949, -0.8384, -0.4594, 2.9742, -1.1313, -2.3144] q2 = [0.2846, -0.3534, -2.2822, -1.2794, -2.6088, -1.298, -1.1206] q3 = [0.281, 0.7666, 0.0846, -1.8104, 2.8603, -1.1027, -3.1227] q4 = [0.0514, -0.3534, -2.8542, -1.7384, -2.95, -1.3661, -0.3652] q5 = [0.2874, -0.0535, -2.3339, -1.2294, -2.8416, -1.3918, -0.9476] q6 = [0.5259, 0.9754, -0.2409, -0.9607, 1.7601, -0.748, -1.6548] q7 = [-0.4355, 1.2821, -0.5184, -1.4671, 2.7845, -0.2334, -2.709] q8 = [-1.0536, -0.3436, -1.6586, -1.4779, -2.7631, -0.4247, -1.6377] q9 = [-0.7221, -0.3534, -1.639, -1.7309, -2.8092, -0.9864, -1.5293] q10 = [-0.3357, -0.1715, -0.3289, -2.0001, 1.6523, -1.9265, 2.5474] q11 = [-0.3888, 1.0477, -1.1405, -0.7096, 0.9253, -0.5049, -0.3575] qs = [q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11] # Collisions s1 = rp.Box(scale=[0.4, 1.4, 0.02], base=sm.SE3(0.75, -0.1, 0.7)) s2 = rp.Box(scale=[1.1, 0.3, 0.02], base=sm.SE3(0.4, -0.65, 0.7)) s3 = rp.Box(scale=[0.02, 0.2, 0.3], base=sm.SE3(0.1, -0.7, 0.85)) s4 = rp.Box(scale=[0.02, 0.2, 0.3], base=sm.SE3(0.3, -0.7, 0.85)) s5 = rp.Box(scale=[0.02, 0.2, 0.3], base=sm.SE3(0.5, -0.7, 0.85)) s6 = rp.Box(scale=[0.42, 0.3, 0.02], base=sm.SE3(0.3, -0.65, 0.71)) s7 = rp.Box(scale=[0.02, 0.1, 0.06], base=sm.SE3(0.1, -0.55, 0.73)) s8 = rp.Box(scale=[0.02, 0.1, 0.06], base=sm.SE3(0.3, -0.55, 0.73))
import time # Configurations q0 = [-0.8452, 0.0813, -1.8312, -1.555, 2.6911, -0.8326, 2.068] q1 = [-0.4134, -0.238, -3.6504, -1.1768, 2.7225, -1.2706, -2.3709] q2 = [-0.9547, 0.3356, -2.3151, -0.9126, 1.8166, -0.8724, -3.1287] q3 = [-0.496, -0.2946, -2.626, -1.5671, -0.9644, -0.5307, 0.5828] q4 = [-0.978, -0.235, -1.3629, -1.282, -2.2903, -0.4913, 0.9081] q5 = [-0.3043, -0.1995, 0.4997, -0.9161, -3.0128, -1.2772, -0.4844] q6 = [-0.0826, -0.3115, -0.7685, -1.0468, -2.8332, -1.2915, 0.7087] q7 = [-0.9493, -0.2259, -1.2924, -1.2902, -2.2911, -0.5655, -2.1449] q8 = [-0.0077, -0.1813, -1.2825, -0.2072, -2.475, -0.3674, -2.5659] qs = [q0, q1, q2, q3, q4, q5, q6, q7, q8] # Collisions s1 = rp.Box(scale=[0.60, 0.71, 0.02], base=sm.SE3(0.30, 0.355, 1.50)) s2 = rp.Box(scale=[0.70, 0.02, 0.90], base=sm.SE3(0.65, 0.01, 1.06)) s3 = rp.Box(scale=[0.59, 0.70, 0.02], base=sm.SE3(0.30, 0.35, 1.23)) s4 = rp.Box(scale=[0.59, 0.70, 0.02], base=sm.SE3(0.30, 0.35, 0.92)) s5 = rp.Box(scale=[0.59, 0.70, 0.02], base=sm.SE3(0.30, 0.35, 0.60)) s6 = rp.Box(scale=[0.02, 0.70, 0.60], base=sm.SE3(0.60, 0.35, 0.30)) s7 = rp.Box(scale=[0.07, 0.03, 0.03], base=sm.SE3(0.63, 0.20, 0.55)) s8 = rp.Box(scale=[0.07, 0.03, 0.03], base=sm.SE3(0.63, 0.50, 0.55))
