def group_param_rot_spec(robo, symo, j, lam, antRj):
"""Internal function. Groups inertia parameters according to the
special rule for a rotational joint.
Notes
=====
robo is the output paramete
"""
chainj = robo.chain(j)
r1, r2, orthog = Transform.find_r12(robo, chainj, antRj, j)
kRj, all_paral = Transform.kRj(robo, antRj, r1, chainj)
Kj = robo.get_inert_param(j)
to_replace = {0, 1, 2, 4, 5, 6, 7}
if Transform.z_paral(kRj):
Kj[0] = 0 # XX
Kj[1] = 0 # XY
Kj[2] = 0 # XZ
Kj[4] = 0 # YZ
to_replace -= {0, 1, 2, 4}
joint_axis = antRj[chainj[-1]].col(2)
if all_paral and robo.G.norm() == sympy.Abs(joint_axis.dot(robo.G)):
Kj[6] = 0 # MX
Kj[7] = 0 # MY
to_replace -= {6, 7}
if j == r1 or(j == r2 and orthog):
Kj[5] += robo.IA[j] # ZZ
robo.IA[j] = 0
for i in to_replace:
Kj[i] = symo.replace(Kj[i], inert_names[i], j)
robo.put_inert_param(Kj, j)
def test_transform(self):
transform = Transform()
transform.translate([1.,1.,1.])
self.polygon.transform(transform)
self.assertTrue(np.allclose(np.matrix('1.,3.,3.,1.; 1. 1. 3. 3. ; 1. 1. 1. 1. '), self.polygon.matrix_of_vertices))
def group_param_prism_spec(robo, symo, j, antRj, antPj):
"""Internal function. Groups inertia parameters according to the
special rule for a prismatic joint.
Notes
=====
robo is the output paramete
"""
chainj = robo.chain(j)
r1, r2, orthog = Transform.find_r12(robo, chainj, antRj, j)
Kj = robo.get_inert_param(j)
kRj, all_paral = Transform.kRj(robo, antRj, r1, chainj)
to_replace = {6, 7, 8, 9}
if r1 < j and j < r2:
if Transform.z_paral(kRj):
Kj[8] = 0 # MZ
for i in (6, 7):
Kj[i] = symo.replace(Kj[i], inert_names[i], j)
robo.MS[robo.ant[j]] += antRj[j]*Matrix([Kj[6], Kj[7], 0])
robo.JJ[2, 2] -= Kj[6]*antPj[j][0] + Kj[7]*antPj[j][1]
Kj[6] = 0 # MX
Kj[7] = 0 # MY
to_replace -= {6, 7, 8}
else:
jar1 = kRj.row(2)
if jar1[2] != 0:
Kj[6] -= jar1[0]/jar1[2]*Kj[8]
Kj[7] -= jar1[1]/jar1[2]*Kj[8]
Kj[8] = 0 # MZ
to_replace -= {8}
elif jar1[0]*jar1[1] != 0:
Kj[6] -= jar1[0]/jar1[1]*Kj[7]
Kj[7] = 0 # MY
to_replace -= {7}
elif jar1[0] != 0:
Kj[7] = 0 # MY
to_replace -= {7}
else:
Kj[6] = 0 # MX
to_replace -= {6}
elif j < r1:
Kj[6] = 0 # MX
Kj[7] = 0 # MY
Kj[8] = 0 # MZ
to_replace -= {6, 7, 8}
#TOD: rewrite
dotGa = Transform.sna(antRj[j])[2].dot(robo.G)
if dotGa == ZERO:
revol_align = robo.ant[robo.ant[j]] == 0 and robo.ant[j] == ZERO
if robo.ant[j] == 0 or revol_align:
Kj[9] += robo.IA[j]
robo.IA[j] = 0
for i in to_replace:
Kj[i] = symo.replace(Kj[i], inert_names[i], j)
robo.put_inert_param(Kj, j)
def draw_docking_station(ax, goal_object='station'):
"""
:param ax:
:param goal_object
"""
obj_tform = Transform.scale(20) @ Transform.translate(-0.85, 0)
if goal_object == 'station':
obj_points = Point.from_list([(0, 1, 1), (2, 1, 1), (2, 0.5, 1),
(0.5, 0.5, 1), (0.5, -0.5, 1),
(2, -0.5, 1), (2, -1, 1), (0, -1, 1)])
obj_points = obj_points.transformed(obj_tform).to_euclidean().T
ax.add_patch(
plt.Polygon(obj_points,
facecolor=colors.to_rgba([0, 0.5, 0.5], alpha=0.5),
edgecolor=[0, 0.5, 0.5],
linewidth=1.5))
elif goal_object == 'coloured_station':
face_colours = [[0.839, 0.153, 0.157], [1.000, 0.895, 0.201],
[0.173, 0.627, 0.173]]
obj_points = Point.from_list([(0, 1, 1), (0, 0.5, 1), (2, 0.5, 1),
(2, 1, 1), (0, -1, 1), (0, -0.5, 1),
(2, -0.5, 1), (2, -1, 1), (0, 0.5, 1),
(0, -0.5, 1), (0.5, -0.5, 1),
(0.5, 0.5, 1)])
obj_points = obj_points.transformed(obj_tform).to_euclidean().T
arm1_points = obj_points[:4]
arm2_points = obj_points[4:8]
back_points = obj_points[8:]
ax.add_patch(
plt.Polygon(back_points,
facecolor=colors.to_rgba(face_colours[1], alpha=0.5),
edgecolor=face_colours[1],
linewidth=1.5))
ax.add_patch(
plt.Polygon(arm1_points,
facecolor=colors.to_rgba(face_colours[0], alpha=0.5),
edgecolor=face_colours[0],
linewidth=1.5))
ax.add_patch(
plt.Polygon(arm2_points,
facecolor=colors.to_rgba(face_colours[2], alpha=0.5),
edgecolor=face_colours[2],
linewidth=1.5))
else:
raise ValueError("Invalid value for goal_object")
def __init__(self, a=1.0, b=1.0, c=0.0, d=0.0, e=0.0, f=-1.0, color=None):
Shape.__init__(self, color)
self.a = a
self.b = b
self.c = c
self.d = d
self.e = e
self.f = f
t = Transform(2 * a, c, 0, c, 2 * b, 0)
self.center = t.inverse() * Vector(-d, -e)
l1, l0 = quadratic(1, 2 * (-a - b), 4 * a * b - c * c)
v = t.eigv()
axes = [v[0] * ((l0 / 2) ** -0.5), v[1] * ((l1 / 2) ** -0.5)]
self.bound = Vector.union(self.center - axes[0] - axes[1],
self.center - axes[0] + axes[1],
self.center + axes[0] - axes[1],
self.center + axes[0] + axes[1])
def update(self, position, angle):
tform = Transform.pose_transform(position, angle)
points = self._points.transformed(tform).to_euclidean().T
self.circle.center = points[0]
self.arrow.xy = points[1:]
if show_range:
self.range_circle.center = points[0]
class TestTransform(unittest.TestCase):
def setUp(self):
self.transform = Transform()
self.translation = [1.,1.,1.]
def test_translation(self):
self.transform.translate(self.translation)
reference = np.matrix('1., 0., 1.; 0., 1., 1.; 0., 0., 1.')
tested = self.transform.matrix
self.assertTrue(np.allclose(reference, tested))
def test_rotation(self):
self.transform.rotate(90.)
reference = np.matrix('0., -1., 0.; 1., 0., 0.; 0., 0., 1.')
self.assertTrue(np.allclose(reference, self.transform.matrix))
def __init__(self, a=1.0, b=1.0, c=0.0, d=0.0, e=0.0, f=-1.0, color=None):
Shape.__init__(self, color)
if c * c - 4 * a * b >= 0:
raise Exception("Not an ellipse")
self.a = a
self.b = b
self.c = c
self.d = d
self.e = e
self.f = f
self.gradient = Transform(2 * a, c, d, c, 2 * b, e)
self.center = self.gradient.inverse() * Vector(0, 0)
y1, y2 = quadratic(b - c * c / 4 * a, e - c * d / 2 * a,
f - d * d / 4 * a)
x1, x2 = quadratic(a - c * c / 4 * b, d - c * e / 2 * b,
f - e * e / 4 * b)
self.bound = AABox.from_vectors(Vector(-(d + c * y1) / 2 * a, y1),
Vector(-(d + c * y2) / 2 * a, y2),
Vector(x1, -(e + c * x1) / 2 * b),
Vector(x2, -(e + c * x2) / 2 * b))
if not self.contains(self.center):
raise Exception("Internal error, center not inside ellipse")
def init_positions(cls,
marxbot,
d_object,
marxbot_rel_pose,
min_distance=8.5):
"""
:param marxbot
:param d_object
:param marxbot_rel_pose: initial pose of the marxbot, relative to the goal pose, expressed as r, theta and
angle, that is position in polar coordinates and orientation
:param min_distance: the minimum distance between the marXbot and any object, that correspond to the radius
of the marXbot, is 8.5 cm.
"""
# The goal pose of the marXbot, defined with respect to the docking station reference frame, has the same y-axis
# of the docking station and the x-axis translated of the radius of the d_object plus a small arbitrary distance.
# The goal angle of the marXbot is 180 degree (π).
increment = d_object.radius + min_distance
x_goal = d_object.position[0] + increment
y_goal = d_object.position[1] + 0
marxbot.goal_position = (x_goal, y_goal)
marxbot.goal_angle = np.pi
# Transform the initial pose, relative to the goal, from polar to cartesian coordinates
r, theta, angle = marxbot_rel_pose
point_G = Point.from_polar(r, theta)
# Transform the cartesian coordinates from the goal to the world reference frame
trasform_D_G = Transform.translate(increment, 0)
trasform_W_D = Transform.pose_transform(d_object.position,
d_object.angle)
point_W = trasform_W_D @ trasform_D_G @ point_G
marxbot.initial_position = tuple(Point.to_euclidean(point_W))
marxbot.position = marxbot.initial_position
marxbot.initial_angle = angle
marxbot.angle = marxbot.initial_angle
marxbot.goal_reached = False
def _jac(robo, symo, n, i, j, chain=None, forced=False, trig_subs=False):
"""
Computes jacobian of frame n (with origin On in Oj) projected to frame i
"""
# symo.write_geom_param(robo, 'Jacobian')
# TODO: Check projection frames, rewrite DGM call for higher efficiency
M = []
if chain is None:
chain = robo.chain(n)
chain.reverse()
# chain_ext = chain + [robo.ant[min(chain)]]
# if not i in chain_ext:
# i = min(chain_ext)
# if not j in chain_ext:
# j = max(chain_ext)
kTj_dict = dgm(robo, symo, chain[0], j, key='left', trig_subs=trig_subs)
kTj_tmp = dgm(robo, symo, chain[-1], j, key='left', trig_subs=trig_subs)
kTj_dict.update(kTj_tmp)
iTk_dict = dgm(robo, symo, i, chain[0], key='right', trig_subs=trig_subs)
iTk_tmp = dgm(robo, symo, i, chain[-1], key='right', trig_subs=trig_subs)
iTk_dict.update(iTk_tmp)
for k in chain:
kTj = kTj_dict[k, j]
iTk = iTk_dict[i, k]
isk, ink, iak = Transform.sna(iTk)
sigm = robo.sigma[k]
if sigm == 1:
dvdq = iak
J_col = dvdq.col_join(Matrix([0, 0, 0]))
elif sigm == 0:
dvdq = kTj[0, 3]*ink-kTj[1, 3]*isk
J_col = dvdq.col_join(iak)
else:
J_col = Matrix([0, 0, 0, 0, 0, 0])
M.append(J_col.T)
Jac = Matrix(M).T
Jac = Jac.applyfunc(symo.simp)
iRj = Transform.R(iTk_dict[i, j])
jTn = dgm(robo, symo, j, n, fast_form=False, trig_subs=trig_subs)
jPn = Transform.P(jTn)
L = -hat(iRj*jPn)
if forced:
symo.mat_replace(Jac, 'J', '', forced)
L = symo.mat_replace(L, 'L', '', forced)
return Jac, L
def __init__(self, a=1.0, b=1.0, c=0.0, d=0.0, e=0.0, f=-1.0, color=None):
Shape.__init__(self, color)
if c*c - 4*a*b >= 0:
raise Exception("Not an ellipse")
self.a = a
self.b = b
self.c = c
self.d = d
self.e = e
self.f = f
self.gradient = Transform(2*a, c, d, c, 2*b, e)
self.center = self.gradient.inverse() * Vector(0, 0)
y1, y2 = quadratic(b-c*c/4*a, e-c*d/2*a, f-d*d/4*a)
x1, x2 = quadratic(a-c*c/4*b, d-c*e/2*b, f-e*e/4*b)
self.bound = AABox.from_vectors(Vector(-(d + c*y1)/2*a, y1),
Vector(-(d + c*y2)/2*a, y2),
Vector(x1, -(e + c*x1)/2*b),
Vector(x2, -(e + c*x2)/2*b))
if not self.contains(self.center):
raise Exception("Internal error, center not inside ellipse")
class Ellipse(Shape):
def __init__(self, a=1.0, b=1.0, c=0.0, d=0.0, e=0.0, f=-1.0, color=None):
Shape.__init__(self, color)
if c * c - 4 * a * b >= 0:
raise Exception("Not an ellipse")
self.a = a
self.b = b
self.c = c
self.d = d
self.e = e
self.f = f
self.gradient = Transform(2 * a, c, d, c, 2 * b, e)
self.center = self.gradient.inverse() * Vector(0, 0)
y1, y2 = quadratic(b - c * c / 4 * a, e - c * d / 2 * a,
f - d * d / 4 * a)
x1, x2 = quadratic(a - c * c / 4 * b, d - c * e / 2 * b,
f - e * e / 4 * b)
self.bound = AABox.from_vectors(Vector(-(d + c * y1) / 2 * a, y1),
Vector(-(d + c * y2) / 2 * a, y2),
Vector(x1, -(e + c * x1) / 2 * b),
Vector(x2, -(e + c * x2) / 2 * b))
if not self.contains(self.center):
raise Exception("Internal error, center not inside ellipse")
def value(self, p):
return self.a*p.x*p.x + self.b*p.y*p.y + self.c*p.x*p.y \
+ self.d*p.x + self.e*p.y + self.f
def contains(self, p):
return self.value(p) < 0
def transform(self, transform):
i = transform.inverse()
((m00, m01, m02), (m10, m11, m12), _) = i.m
aa = self.a * m00 * m00 + self.b * m10 * m10 + self.c * m00 * m10
bb = self.a * m01 * m01 + self.b * m11 * m11 + self.c * m01 * m11
cc = 2*self.a*m00*m01 + 2*self.b*m10*m11 \
+ self.c*(m00*m11 + m01*m10)
dd = 2*self.a*m00*m02 + 2*self.b*m10*m12 \
+ self.c*(m00*m12 + m02*m10) + self.d*m00 + self.e*m10
ee = 2*self.a*m01*m02 + 2*self.b*m11*m12 \
+ self.c*(m01*m12 + m02*m11) + self.d*m01 + self.e*m11
ff = self.a*m02*m02 + self.b*m12*m12 + self.c*m02*m12 \
+ self.d*m02 + self.e*m12 + self.f
return Ellipse(aa, bb, cc, dd, ee, ff, color=self.color)
def intersections(self, c, p):
# returns the two intersections of the line through c and p
# and the ellipse. Defining a line as a function of a single
# parameter u, x(u) = c.x + u * (p.x - c.x), (and same for y)
# this simply solves the quadratic equation f(x(u), y(u)) = 0
pc = p - c
u2 = self.a * pc.x**2 + self.b * pc.y**2 + self.c * pc.x * pc.y
u1 = 2*self.a*c.x*pc.x + 2*self.b*c.y*pc.y \
+ self.c*c.y*pc.x + self.c*c.x*pc.y + self.d*pc.x \
+ self.e*pc.y
u0 = self.a*c.x**2 + self.b*c.y**2 + self.c*c.x*c.y \
+ self.d*c.x + self.e*c.y + self.f
try:
sols = quadratic(u2, u1, u0)
except ValueError:
raise Exception("Internal error, solutions be real numbers")
return c + pc * sols[0], c + pc * sols[1]
def signed_distance_bound(self, p):
v = self.value(p)
if v == 0:
return 0
elif v < 0:
# if inside the ellipse, create an inscribed quadrilateral
# that contains the given point and use the minimum distance
# from the point to the quadrilateral as a bound. Since
# the quadrilateral lies entirely inside the ellipse, the
# distance from the point to the ellipse must be smaller.
v0, v2 = self.intersections(p, p + Vector(1, 0))
v1, v3 = self.intersections(p, p + Vector(0, 1))
return abs(ConvexPoly([v0, v1, v2, v3]).signed_distance_bound(p))
else:
c = self.center
crossings = self.intersections(c, p)
# the surface point we want is the one closest to p
if (p - crossings[0]).length() < (p - crossings[1]).length():
surface_pt = crossings[0]
else:
surface_pt = crossings[1]
# n is the normal at surface_pt
n = self.gradient * surface_pt
n = n * (1.0 / n.length())
# returns the length of the projection of p - surface_pt
# along the normal
return -abs(n.dot(p - surface_pt))
class Ellipse(Shape):
def __init__(self, a=1.0, b=1.0, c=0.0, d=0.0, e=0.0, f=-1.0, color=None):
Shape.__init__(self, color)
if c*c - 4*a*b >= 0:
raise Exception("Not an ellipse")
self.a = a
self.b = b
self.c = c
self.d = d
self.e = e
self.f = f
self.gradient = Transform(2*a, c, d, c, 2*b, e)
self.center = self.gradient.inverse() * Vector(0, 0)
y1, y2 = quadratic(b-c*c/4*a, e-c*d/2*a, f-d*d/4*a)
x1, x2 = quadratic(a-c*c/4*b, d-c*e/2*b, f-e*e/4*b)
self.bound = AABox.from_vectors(Vector(-(d + c*y1)/2*a, y1),
Vector(-(d + c*y2)/2*a, y2),
Vector(x1, -(e + c*x1)/2*b),
Vector(x2, -(e + c*x2)/2*b))
if not self.contains(self.center):
raise Exception("Internal error, center not inside ellipse")
def value(self, p):
return self.a*p.x*p.x + self.b*p.y*p.y + self.c*p.x*p.y \
+ self.d*p.x + self.e*p.y + self.f
def contains(self, p):
return self.value(p) < 0
def transform(self, transform):
i = transform.inverse()
((m00, m01, m02), (m10, m11, m12),_) = i.m
aa = self.a*m00*m00 + self.b*m10*m10 + self.c*m00*m10
bb = self.a*m01*m01 + self.b*m11*m11 + self.c*m01*m11
cc = 2*self.a*m00*m01 + 2*self.b*m10*m11 \
+ self.c*(m00*m11 + m01*m10)
dd = 2*self.a*m00*m02 + 2*self.b*m10*m12 \
+ self.c*(m00*m12 + m02*m10) + self.d*m00 + self.e*m10
ee = 2*self.a*m01*m02 + 2*self.b*m11*m12 \
+ self.c*(m01*m12 + m02*m11) + self.d*m01 + self.e*m11
ff = self.a*m02*m02 + self.b*m12*m12 + self.c*m02*m12 \
+ self.d*m02 + self.e*m12 + self.f
return Ellipse(aa, bb, cc, dd, ee, ff, color=self.color)
def intersections(self, c, p):
# returns the two intersections of the line through c and p
# and the ellipse. Defining a line as a function of a single
# parameter u, x(u) = c.x + u * (p.x - c.x), (and same for y)
# this simply solves the quadratic equation f(x(u), y(u)) = 0
pc = p - c
u2 = self.a*pc.x**2 + self.b*pc.y**2 + self.c*pc.x*pc.y
u1 = 2*self.a*c.x*pc.x + 2*self.b*c.y*pc.y \
+ self.c*c.y*pc.x + self.c*c.x*pc.y + self.d*pc.x \
+ self.e*pc.y
u0 = self.a*c.x**2 + self.b*c.y**2 + self.c*c.x*c.y \
+ self.d*c.x + self.e*c.y + self.f
try:
sols = quadratic(u2, u1, u0)
except ValueError:
raise Exception("Internal error, solutions be real numbers")
return c+pc*sols[0], c+pc*sols[1]
def signed_distance_bound(self, p):
v = self.value(p)
if v == 0:
return 0
elif v < 0:
# if inside the ellipse, create an inscribed quadrilateral
# that contains the given point and use the minimum distance
# from the point to the quadrilateral as a bound. Since
# the quadrilateral lies entirely inside the ellipse, the
# distance from the point to the ellipse must be smaller.
v0, v2 = self.intersections(p, p + Vector(1, 0))
v1, v3 = self.intersections(p, p + Vector(0, 1))
return abs(ConvexPoly([v0,v1,v2,v3]).signed_distance_bound(p))
else:
c = self.center
crossings = self.intersections(c, p)
# the surface point we want is the one closest to p
if (p - crossings[0]).length() < (p - crossings[1]).length():
surface_pt = crossings[0]
else:
surface_pt = crossings[1]
# n is the normal at surface_pt
n = self.gradient * surface_pt
n = n * (1.0 / n.length())
# returns the length of the projection of p - surface_pt
# along the normal
return -abs(n.dot(p - surface_pt))
def f(translation, rotation):
t = Transform()
t.rotate(rotation)
t.translate(translation)
return area_of_intersection(t)
def setUp(self):
self.transform = Transform()
self.translation = [1.,1.,1.]
