From bc7bf8c00ddeb73ca0857201d4d83db5278121de Mon Sep 17 00:00:00 2001 From: levdoescode Date: Thu, 15 Dec 2022 23:52:52 -0500 Subject: [PATCH] Geneome and URDF class completed --- .../Week 8/Files/genome.py | 266 ++++++++++++++++++ 1 file changed, 266 insertions(+) create mode 100644 CM3020 Artificial Intelligence/Week 8/Files/genome.py diff --git a/CM3020 Artificial Intelligence/Week 8/Files/genome.py b/CM3020 Artificial Intelligence/Week 8/Files/genome.py new file mode 100644 index 0000000..a9b3ebc --- /dev/null +++ b/CM3020 Artificial Intelligence/Week 8/Files/genome.py @@ -0,0 +1,266 @@ +import numpy as np +import copy + + +class Genome(): + @staticmethod + def get_random_gene(length): + gene = np.array([np.random.random() for i in range(length)]) + return gene + + @staticmethod + def get_random_genome(gene_length, gene_count): + genome = [Genome.get_random_gene(gene_length) + for i in range(gene_count)] + return genome + + @staticmethod + def get_gene_spec(): + gene_spec = {"link-shape": {"scale": 1}, + "link-length": {"scale": 1}, + "link-radius": {"scale": 1}, + "link-recurrence": {"scale": 4}, + "link-mass": {"scale": 1}, + "joint-type": {"scale": 1}, + "joint-parent": {"scale": 1}, + "joint-axis-xyz": {"scale": 1}, + "joint-origin-rpy-1": {"scale": np.pi * 2}, + "joint-origin-rpy-2": {"scale": np.pi * 2}, + "joint-origin-rpy-3": {"scale": np.pi * 2}, + "joint-origin-xyz-1": {"scale": 1}, + "joint-origin-xyz-2": {"scale": 1}, + "joint-origin-xyz-3": {"scale": 1}, + "control-waveform": {"scale": 1}, + "control-amp": {"scale": 0.25}, + "control-freq": {"scale": 1} + } + ind = 0 + for key in gene_spec.keys(): + gene_spec[key]["ind"] = ind + ind = ind + 1 + return gene_spec + + @staticmethod + def get_gene_dict(gene, spec): + gdict = {} + for key in spec: + ind = spec[key]["ind"] + scale = spec[key]["scale"] + gdict[key] = gene[ind] * scale + return gdict + + @staticmethod + def get_genome_dicts(genome, spec): + gdicts = [] + for gene in genome: + gdicts.append(Genome.get_gene_dict(gene, spec)) + return gdicts + + @staticmethod + def expandLinks(parent_link, uniq_parent_name, flat_links, exp_links): + children = [l for l in flat_links if l.parent_name == parent_link.name] + sibling_ind = 1 + for c in children: + for r in range(int(c.recur)): + sibling_ind = sibling_ind + 1 + c_copy = copy.copy(c) + c_copy.parent_name = uniq_parent_name + uniq_name = c_copy.name + str(len(exp_links)) + #print("exp: ", c.name, " -> ", uniq_name) + c_copy.name = uniq_name + c_copy.sibling_ind = sibling_ind + exp_links.append(c_copy) + assert c.parent_name != c.name, "Genome::expandLinks: link joined to itself: " + \ + c.name + " joins " + c.parent_name + Genome.expandLinks(c, uniq_name, flat_links, exp_links) + + @staticmethod + def genome_to_links(gdicts): + links = [] + link_ind = 0 + parent_names = [str(link_ind)] + for gdict in gdicts: + link_name = str(link_ind) + parent_ind = gdict["joint-parent"] * len(parent_names) + parent_name = parent_names[int(parent_ind)] + #print("available parents: ", parent_names, "chose", parent_name) + recur = gdict["link-recurrence"] + link = URDFLink(name=link_name, + parent_name=parent_name, + recur=recur+1, + link_length=gdict["link-length"], + link_radius=gdict["link-radius"], + link_mass=gdict["link-mass"], + joint_type=gdict["joint-type"], + joint_parent=gdict["joint-parent"], + joint_axis_xyz=gdict["joint-axis-xyz"], + joint_origin_rpy_1=gdict["joint-origin-rpy-1"], + joint_origin_rpy_2=gdict["joint-origin-rpy-2"], + joint_origin_rpy_3=gdict["joint-origin-rpy-3"], + joint_origin_xyz_1=gdict["joint-origin-xyz-1"], + joint_origin_xyz_2=gdict["joint-origin-xyz-2"], + joint_origin_xyz_3=gdict["joint-origin-xyz-3"], + control_waveform=gdict["control-waveform"], + control_amp=gdict["control-amp"], + control_freq=gdict["control-freq"]) + links.append(link) + if link_ind != 0: # don't re-add the first link + parent_names.append(link_name) + link_ind = link_ind + 1 + + # now just fix the first link so it links to nothing + links[0].parent_name = "None" + return links + + +class URDFLink: + def __init__(self, name, parent_name, recur, + link_length=0.1, + link_radius=0.1, + link_mass=0.1, + joint_type=0.1, + joint_parent=0.1, + joint_axis_xyz=0.1, + joint_origin_rpy_1=0.1, + joint_origin_rpy_2=0.1, + joint_origin_rpy_3=0.1, + joint_origin_xyz_1=0.1, + joint_origin_xyz_2=0.1, + joint_origin_xyz_3=0.1, + control_waveform=0.1, + control_amp=0.1, + control_freq=0.1): + self.name = name + self.parent_name = parent_name + self.recur = recur + self.link_length = link_length + self.link_radius = link_radius + self.link_mass = link_mass + self.joint_type = joint_type + self.joint_parent = joint_parent + self.joint_axis_xyz = joint_axis_xyz + self.joint_origin_rpy_1 = joint_origin_rpy_1 + self.joint_origin_rpy_2 = joint_origin_rpy_2 + self.joint_origin_rpy_3 = joint_origin_rpy_3 + self.joint_origin_xyz_1 = joint_origin_xyz_1 + self.joint_origin_xyz_2 = joint_origin_xyz_2 + self.joint_origin_xyz_3 = joint_origin_xyz_3 + self.control_waveform = control_waveform + self.control_amp = control_amp + self.control_freq = control_freq + self.sibling_ind = 1 + + def to_link_element(self, adom): + # + # + # + # + # + # + # + # + # + # + # + # + # + # + # + # + + link_tag = adom.createElement("link") + link_tag.setAttribute("name", self.name) + vis_tag = adom.createElement("visual") + geom_tag = adom.createElement("geometry") + cyl_tag = adom.createElement("cylinder") + cyl_tag.setAttribute("length", str(self.link_length)) + cyl_tag.setAttribute("radius", str(self.link_radius)) + + geom_tag.appendChild(cyl_tag) + vis_tag.appendChild(geom_tag) + + coll_tag = adom.createElement("collision") + c_geom_tag = adom.createElement("geometry") + c_cyl_tag = adom.createElement("cylinder") + c_cyl_tag.setAttribute("length", str(self.link_length)) + c_cyl_tag.setAttribute("radius", str(self.link_radius)) + + c_geom_tag.appendChild(c_cyl_tag) + coll_tag.appendChild(c_geom_tag) + + # + # + # + # + inertial_tag = adom.createElement("inertial") + mass_tag = adom.createElement("mass") + # pi r^2 * height + mass = np.pi * (self.link_radius * self.link_radius) * self.link_length + mass_tag.setAttribute("value", str(mass)) + inertia_tag = adom.createElement("inertia") + # + inertia_tag.setAttribute("ixx", "0.03") + inertia_tag.setAttribute("iyy", "0.03") + inertia_tag.setAttribute("izz", "0.03") + inertia_tag.setAttribute("ixy", "0") + inertia_tag.setAttribute("ixz", "0") + inertia_tag.setAttribute("iyx", "0") + inertial_tag.appendChild(mass_tag) + inertial_tag.appendChild(inertia_tag) + + link_tag.appendChild(vis_tag) + link_tag.appendChild(coll_tag) + link_tag.appendChild(inertial_tag) + + return link_tag + + def to_joint_element(self, adom): + # + # + # + # + # + # + # + joint_tag = adom.createElement("joint") + joint_tag.setAttribute("name", self.name + "_to_" + self.parent_name) + if self.joint_type >= 0.5: + joint_tag.setAttribute("type", "revolute") + else: + joint_tag.setAttribute("type", "revolute") + parent_tag = adom.createElement("parent") + parent_tag.setAttribute("link", self.parent_name) + child_tag = adom.createElement("child") + child_tag.setAttribute("link", self.name) + axis_tag = adom.createElement("axis") + if self.joint_axis_xyz <= 0.33: + axis_tag.setAttribute("xyz", "1 0 0") + if self.joint_axis_xyz > 0.33 and self.joint_axis_xyz <= 0.66: + axis_tag.setAttribute("xyz", "0 1 0") + if self.joint_axis_xyz > 0.66: + axis_tag.setAttribute("xyz", "0 0 1") + + limit_tag = adom.createElement("limit") + # effort upper lower velocity + limit_tag.setAttribute("effort", "1") + limit_tag.setAttribute("upper", "-3.1415") + limit_tag.setAttribute("lower", "3.1415") + limit_tag.setAttribute("velocity", "1") + # + orig_tag = adom.createElement("origin") + + rpy1 = self.joint_origin_rpy_1 * self.sibling_ind + + rpy = str(rpy1) + " " + str(self.joint_origin_rpy_2) + \ + " " + str(self.joint_origin_rpy_3) + orig_tag.setAttribute("rpy", rpy) + xyz = str(self.joint_origin_xyz_1) + " " + \ + str(self.joint_origin_xyz_2) + " " + str(self.joint_origin_xyz_3) + orig_tag.setAttribute("xyz", xyz) + + joint_tag.appendChild(parent_tag) + joint_tag.appendChild(child_tag) + joint_tag.appendChild(axis_tag) + joint_tag.appendChild(limit_tag) + joint_tag.appendChild(orig_tag) + return joint_tag