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.]