├── .gitignore ├── README.md ├── bin └── blend2cpp ├── scripts └── blender_export.py └── src ├── __init__.py ├── blender_utils.py ├── generator ├── __init__.py ├── cpp.py ├── generator.py ├── ik.py └── ik_utils.py ├── kinematics ├── __init__.py ├── chain.py ├── chain_element.py └── matrix_chain_element.py ├── main.py └── test.py /.gitignore: -------------------------------------------------------------------------------- 1 | *.pyc 2 | *~ 3 | \#* 4 | a.out 5 | *.c 6 | *.cpp 7 | *.hpp 8 | *.o 9 | blender.out.json 10 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ik 2 | 3 | This tool is a part of the [Aversive++ project](https://github.com/AversivePlusPlus/AversivePlusPlus). 4 | 5 | # Description 6 | 7 | This is a python tool that generates C++ code to solve inverse kinematics problems. 8 | The generated C++ code can then be used in microcontrollers projects. 9 | 10 | The tool can import kinematics chain from `blend` files. 11 | 12 | # Installation 13 | 14 | The tool cannot be installed yet. But you can test it anyway ! 15 | 16 | # Use 17 | 18 | To list available endpoints in the blend file : 19 | ```bash 20 | ./bin/blend2cpp 21 | ``` 22 | 23 | To generate C++ code from an endpoint : 24 | ```bash 25 | ./bin/blend2cpp 26 | ``` 27 | -------------------------------------------------------------------------------- /bin/blend2cpp: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import sys 4 | import os 5 | 6 | cur_path = os.path.dirname(os.path.abspath(__file__)) 7 | path = cur_path + "/../src" 8 | 9 | sys.path.append(path) 10 | from main import main 11 | main(sys.argv) -------------------------------------------------------------------------------- /scripts/blender_export.py: -------------------------------------------------------------------------------- 1 | """ 2 | Copyright (c) 2015, Xenomorphales 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | * Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | * Neither the name of Aversive++ nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | """ 30 | 31 | import bpy 32 | import json 33 | 34 | def bone_matrix(bone): 35 | if bone.parent == None: 36 | return bone.matrix_local 37 | else: 38 | return bone.parent.matrix_local.inverted() * bone.matrix_local 39 | 40 | def find(bone, func): 41 | func(bone) 42 | for b in bone.children: 43 | find(b, func) 44 | 45 | def matrix2cas(mat): 46 | ret = [] 47 | for row in mat: 48 | ret_row = [] 49 | for case in row: 50 | ret_row.append(case) 51 | ret.append(ret_row) 52 | return ret; 53 | 54 | def name2cxx(name): 55 | return str(name).replace(".", "_") 56 | 57 | def print_variable(cond): 58 | ret = "static constexpr bool VARIABLE = " 59 | if cond: 60 | ret += "true" 61 | else: 62 | ret += "false" 63 | print(ret + ";") 64 | 65 | def print_parent(obj): 66 | if obj.parent != None: 67 | if obj.parent_type == "BONE": 68 | pname = name2cxx(obj.parent.name) + "/" 69 | pname += name2cxx(obj.parent_bone) 70 | return name2cxx(pname) 71 | else: 72 | return name2cxx(obj.parent.name) 73 | else: 74 | return None 75 | 76 | def print_bone_parent(obj, armature_name, default_parent = None): 77 | if obj.parent != None: 78 | return armature_name + "/" + name2cxx(obj.parent.name) 79 | else: 80 | return default_parent 81 | 82 | def print_pose_bone_parent(obj, armature_name, default_parent = None): 83 | if obj.parent != None: 84 | return armature_name + "/" + name2cxx(obj.parent.name) + "/last" 85 | else: 86 | return default_parent 87 | 88 | def get_links_from_bone(bone, armature_name, ret_list = []): 89 | ret = {} 90 | ret["name"] = armature_name + "/" + name2cxx(bone.name) 91 | #ret["is_variable"] = False 92 | ret["parent"] = print_bone_parent(bone, armature_name) 93 | #ret["matrix"] = matrix2cas(bone_matrix(bone)) 94 | ret_list.append(ret) 95 | return ret_list 96 | 97 | def get_links_from_armature(armature, ret_list = []): 98 | ret = {} 99 | ret["name"] = "armatures/" + name2cxx(armature.name) 100 | ret["parent"] = None 101 | ret_list.append(ret) 102 | #ret["is_variable"] = False 103 | #ret["bones"] = [] 104 | for b in armature.bones: 105 | ret_list = get_links_from_bone(b, ret["name"], ret_list) 106 | return ret_list 107 | 108 | def print_dof_matrix(name): 109 | ret = [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]] 110 | if name == "rx": 111 | ret[1][1] = "cos(x)" 112 | ret[1][2] = "-sin(x)" 113 | ret[2][2] = "cos(x)" 114 | ret[2][1] = "sin(x)" 115 | elif name == "ry": 116 | ret[0][0] = "cos(x)" 117 | ret[0][2] = "-sin(x)" 118 | ret[2][2] = "cos(x)" 119 | ret[2][0] = "sin(x)" 120 | elif name == "rz": 121 | ret[0][0] = "cos(x)" 122 | ret[0][1] = "-sin(x)" 123 | ret[1][1] = "cos(x)" 124 | ret[1][0] = "sin(x)" 125 | return ret 126 | 127 | def get_link_from_dof(name, prefix, cond, last, ret_list = []): 128 | if(cond): 129 | ret = {} 130 | ret["name"] = prefix + "/" + name 131 | ret["is_variable"] = True 132 | ret["parent"] = last["name"] 133 | ret["matrix"] = print_dof_matrix(name) 134 | ret_list.append(ret) 135 | return (ret, ret_list) 136 | else: 137 | return (last, ret_list) 138 | 139 | def find_bone_armature(bone): 140 | for a in bpy.data.armatures: 141 | for b in a.bones: 142 | if b == bone: 143 | return (a, b) 144 | return (None, None) 145 | 146 | def print_endpoint(bone): 147 | ret = {} 148 | ret["name"] = "endpoint" 149 | ret["is_variable"] = False 150 | ret["parent"] = "last" 151 | mat = bone.matrix_local.inverted() * bone.tail_local 152 | return ret 153 | 154 | def get_links_from_pose_bone(pose_bone, armature_name, ret_list = []): 155 | ret = {} 156 | ret["name"] = armature_name + "/" + name2cxx(pose_bone.name) 157 | (a, b) = find_bone_armature(pose_bone.bone) 158 | ret["parent"] = print_pose_bone_parent(pose_bone, name2cxx(armature_name), name2cxx(armature_name)) 159 | ret["matrix"] = matrix2cas(bone_matrix(b)) 160 | ret_list.append(ret) 161 | ret["is_variable"] = False 162 | last = ret 163 | (last, ret_list) = get_link_from_dof("rx", ret["name"], not pose_bone.lock_rotation[0], last, ret_list) 164 | (last, ret_list) = get_link_from_dof("ry", ret["name"], not pose_bone.lock_rotation[1], last, ret_list) 165 | (last, ret_list) = get_link_from_dof("rz", ret["name"], not pose_bone.lock_rotation[2], last, ret_list) 166 | last_ret = {} 167 | last_ret["name"] = ret["name"] + "/last" 168 | last_ret["parent"] = last["name"] 169 | last_ret["matrix"] = [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]] 170 | last_ret["is_variable"] = False 171 | ret_list.append(last_ret) 172 | endpoint = {} 173 | endpoint["name"] = ret["name"] + "/endpoint" 174 | endpoint["parent"] = last_ret["name"] 175 | endpoint["matrix"] = list(map(lambda x: [x], (b.matrix_local.inverted()*b.tail_local)[:]))+[[1]] 176 | endpoint["is_variable"] = False 177 | ret_list.append(endpoint) 178 | return ret_list 179 | 180 | def get_links_from_object(obj, ret_list = []): 181 | ret = {} 182 | ret["name"] = name2cxx(obj.name) 183 | ret["is_variable"] = False 184 | ret["parent"] = print_parent(obj) 185 | ret["matrix"] = matrix2cas(obj.matrix_local) 186 | ret_list.append(ret) 187 | if obj.pose != None: 188 | for b in obj.pose.bones: 189 | ret_list = get_links_from_pose_bone(b, ret["name"], ret_list) 190 | return ret_list 191 | 192 | def get_links(ret_list = []): 193 | for o in bpy.data.objects: 194 | ret_list = get_links_from_object(o, ret_list) 195 | return ret_list 196 | 197 | 198 | def check_invalid_parent(links): 199 | valid = set([None]) 200 | parents = set() 201 | for l in links: 202 | valid.add(l["name"]) 203 | for l in links: 204 | parents.add(l["parent"]) 205 | return parents - valid 206 | 207 | def check_doubles(links): 208 | names = set() 209 | for l in links: 210 | if l["name"] not in names: 211 | names.add(l["name"]) 212 | else: 213 | return True 214 | return False 215 | 216 | def get_chains(links): 217 | ret = [] 218 | for l in links: 219 | chain = [] 220 | parent = l 221 | while parent != None: 222 | chain = [parent["name"]] + chain 223 | next = None 224 | for p in links: 225 | if p["name"] == parent["parent"]: 226 | next = p 227 | parent = next 228 | ret.append(chain) 229 | return ret 230 | 231 | def check_missing_matrix(links): 232 | ret = set() 233 | for l in links: 234 | if "matrix" not in l: 235 | ret.add(l["name"]) 236 | return ret 237 | 238 | def check_missing_is_variable(links): 239 | ret = set() 240 | for l in links: 241 | if "is_variable" not in l: 242 | ret.add(l["name"]) 243 | return ret 244 | 245 | 246 | if __name__ == '__main__': 247 | links = get_links() 248 | assert(check_invalid_parent(links) == set()) 249 | assert(check_doubles(links) == False) 250 | assert(check_missing_matrix(links) == set()) 251 | assert(check_missing_is_variable(links) == set()) 252 | f = open("blender.out.json", "w+") 253 | f.write(json.dumps(links, indent=4)) 254 | f.close() 255 | -------------------------------------------------------------------------------- /src/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AversivePlusPlus/ik/4538800860f646a793b74541e35fb85a8e9e8f00/src/__init__.py -------------------------------------------------------------------------------- /src/blender_utils.py: -------------------------------------------------------------------------------- 1 | """ 2 | Copyright (c) 2015, Xenomorphales 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | * Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | * Neither the name of Aversive++ nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | """ 30 | 31 | from kinematics import * 32 | import sympy 33 | import json 34 | import subprocess 35 | import os 36 | 37 | def call_blender_export(path): 38 | python_script = os.path.abspath(os.path.dirname(__file__)) + "/../scripts/blender_export.py" 39 | command = ["blender", path, "--background", "--python", python_script] 40 | subprocess.call(command) 41 | 42 | def read_json(path): 43 | f = open(path, "r") 44 | ret = json.loads(f.read()) 45 | f.close() 46 | return ret 47 | 48 | def list2dict(json_data): 49 | ret = {} 50 | for e in json_data: 51 | ret[e['name']] = e 52 | return ret 53 | 54 | def extract_chain(json_data, endpoint): 55 | json_dict = list2dict(json_data) 56 | elems = [] 57 | cur = endpoint 58 | while cur != None: 59 | edata = json_dict[cur] 60 | if edata['is_variable']: 61 | e = VariableMatrixChainElement(edata['name'], edata['parent'], edata['matrix'], [sympy.Symbol('x')]) 62 | elems = [e] + elems 63 | else: 64 | e = ConstantMatrixChainElement(edata['name'], edata['parent'], edata['matrix']) 65 | elems = [e] + elems 66 | cur = edata['parent'] 67 | return Chain(endpoint, elems) 68 | -------------------------------------------------------------------------------- /src/generator/__init__.py: -------------------------------------------------------------------------------- 1 | from ik import * 2 | -------------------------------------------------------------------------------- /src/generator/cpp.py: -------------------------------------------------------------------------------- 1 | from generator import Generator 2 | 3 | def indent(multiline_string): 4 | lines = multiline_string.split('\n') 5 | ret = [] 6 | for l in lines: 7 | ret.append(" " + l) 8 | return "\n".join(ret) 9 | 10 | class CppMatrixType: 11 | def __init__(self, rows, cols): 12 | self.rows = rows 13 | self.cols = cols 14 | def __str__(self): 15 | return "double[{r}*{c}]".format( 16 | r=self.rows, 17 | c=self.cols 18 | ) 19 | 20 | class CppGenerator(Generator): 21 | def matrix_type(self, rows, cols): 22 | return CppMatrixType(rows, cols) 23 | real_type = "double" 24 | integer_type = "int" 25 | void_type = "void" 26 | def param(self, name, type, default_value = None): 27 | if default_value == None: 28 | return "{type} {name}".format( 29 | name=name, 30 | type=str(type), 31 | ) 32 | return "{type} {name} = {val}".format( 33 | name=name, 34 | type=str(type), 35 | val=str(default_value) 36 | ) 37 | def ref_type(self, type): 38 | return "{type}&".format(type=str(type)) 39 | def const_type(self, type): 40 | return "const {type}".format(type=str(type)) 41 | def list_type(self, type): 42 | return "{type}*".format(type=str(type)) 43 | def call(self, name, params): 44 | return "{name}({params})".format( 45 | name=name, 46 | params=", ".join(map(lambda p: str(p), params)) 47 | ) 48 | def assign(self, var, val): 49 | return "{var} = {val}".format( 50 | var=var, 51 | val=val 52 | ) 53 | def matrix_get(self, name, type, i,j): 54 | return "{matrix}[{index}]".format( 55 | matrix = name, 56 | index = str(i*type.cols+j), 57 | ) 58 | def nested(self, *names): 59 | return "::".join(names) 60 | def list_get(self, name, i): 61 | raise NotImplementedError() 62 | 63 | class CppBlockGenerator(CppGenerator): 64 | def __init__(self, header): 65 | self.source = [] 66 | self.header = header 67 | def __enter__(self): 68 | return self 69 | def __exit__(self, type, value, traceback): 70 | pass 71 | def add_instruction(self, instr): 72 | self.source += [instr + ";"] 73 | return self 74 | def add_variable(self, name, type, default_value = None): 75 | self.source += [ self.param(name, type, default_value) + ";" ] 76 | return self 77 | def add_constant(self, name, type, value): 78 | return self.add_variable(name, self.const_type(type), value) 79 | def define_if_block(self, cond): 80 | header = "if({})".format(cond) 81 | ret = CppBlockGenerator(header) 82 | self.source += [ret] 83 | return ret 84 | def define_for_block(self, size): 85 | template = "for({init} ; {end} ; {inc})" 86 | header = template.format( 87 | init = "int i = 0", 88 | end = "i < " + str(size), 89 | inc = "i++" 90 | ) 91 | ret = CppBlockGenerator(header) 92 | self.source += [ret] 93 | return ret 94 | def __str__(self): 95 | ret = [self.header + " {"] 96 | ret += [indent("\n".join(map(lambda i: str(i), self.source)))] 97 | ret += ["}"] 98 | return "\n".join(ret) 99 | 100 | class CppModuleGenerator(CppGenerator): 101 | def __init__(self, name): 102 | self.name = name 103 | self.header = [] 104 | self.source = [] 105 | def __enter__(self): 106 | return self 107 | def __exit__(self, type, value, traceback): 108 | pass 109 | def add_constant(self, name, type, value): 110 | template = "#define {name} (({type}){value})" 111 | self.header += [ 112 | template.format(name=name, type=str(type), value=str(value)) 113 | ] 114 | return self 115 | def add_instruction(self, instr): 116 | self.header += [instr + ";"] 117 | return self 118 | def define_function(self, name, ret_type, params): 119 | template = "{ret} {name}({params})" 120 | header_signature = template.format( 121 | ret=str(ret_type), 122 | name=name, 123 | params=", ".join(params) 124 | ) 125 | signature = template.format( 126 | ret=str(ret_type), 127 | name=self.nested(self.name, name), 128 | params=", ".join(params) 129 | ) 130 | ret = CppBlockGenerator(signature) 131 | self.header += [header_signature + ";"] 132 | self.source += ["", ret] 133 | return ret 134 | def define_main_function(self): 135 | return self.define_function( 136 | "main", 137 | self.integer_type, 138 | [ 139 | self.param("", self.integer_type), 140 | self.param("", "char**") 141 | ] 142 | ) 143 | def __str__(self): 144 | ret = ["// MODULE : " + self.name] 145 | if self.header != []: 146 | ret += ["//// HEADER"] 147 | ret += ["#ifndef IK_{}_HPP".format(self.name.upper())] 148 | ret += ["#define IK_{}_HPP".format(self.name.upper()), ""] 149 | ret += ["namespace {} {{".format(self.name), ""] 150 | for i in self.header: 151 | ret += [str(i)] 152 | ret += ["", "}", ""] 153 | ret += ["#endif//IK_{}_HPP".format(self.name.upper())] 154 | ret += [""] 155 | if self.source != []: 156 | ret += ["//// SOURCE"] 157 | ret += ["//#include <{}.hpp>".format(self.name)] 158 | ret += ["#include "] 159 | for i in self.source: 160 | ret += [str(i)] 161 | ret += [""] 162 | return "\n".join(ret) 163 | def __iter__(self): 164 | if self.header != []: 165 | yield self.name + ".hpp" 166 | if self.source != []: 167 | yield self.name + ".cpp" 168 | def __getitem__(self, fname): 169 | ret = [] 170 | if fname == self.name + ".hpp": 171 | ret += ["#ifndef IK_{}_HPP".format(self.name.upper())] 172 | ret += ["#define IK_{}_HPP".format(self.name.upper()), ""] 173 | ret += ["namespace {} {{".format(self.name), ""] 174 | for i in self.header: 175 | ret += [str(i)] 176 | ret += ["", "}", ""] 177 | ret += ["#endif//IK_{}_HPP".format(self.name.upper())] 178 | ret += [""] 179 | elif fname == self.name + ".cpp": 180 | ret += ["#include <{}.hpp>".format(self.name)] 181 | ret += ["#include "] 182 | for i in self.source: 183 | ret += [str(i)] 184 | ret += [""] 185 | else: 186 | raise Exception() 187 | return "\n".join(ret) 188 | 189 | class CppProjectGenerator(CppModuleGenerator): 190 | def __init__(self): 191 | CppModuleGenerator.__init__(self, "main") 192 | self.modules = [] 193 | def define_module(self, name): 194 | ret = CppModuleGenerator(name) 195 | self.modules += [ret] 196 | return ret 197 | def __str__(self): 198 | ret = "" 199 | ret += CppModuleGenerator.__str__(self) 200 | ret += '\n' 201 | for m in self.modules: 202 | ret += "////////////////////////////////\n" 203 | ret += str(m) 204 | ret += '\n' 205 | return ret 206 | def __iter__(self): 207 | for e in CppModuleGenerator.__iter__(self): 208 | yield e 209 | for m in self.modules: 210 | for e in m: 211 | yield e 212 | def __getitem__(self, fname): 213 | for e in CppModuleGenerator.__iter__(self): 214 | if e == fname: 215 | return CppModuleGenerator.__getitem__(self, fname) 216 | for m in self.modules: 217 | for e in m: 218 | if e == fname: 219 | return m[e] 220 | -------------------------------------------------------------------------------- /src/generator/generator.py: -------------------------------------------------------------------------------- 1 | 2 | class NotImplementedError(Exception): 3 | pass 4 | 5 | class Generator: 6 | # TYPE 7 | def matrix_type(self, rows, cols): 8 | raise NotImplementedError() 9 | real_type = None 10 | integer_type = None 11 | void_type = None 12 | # DEFINE 13 | def define_module(self, name): 14 | raise NotImplementedError() 15 | def define_function(self, name, ret_type, params): 16 | raise NotImplementedError() 17 | def define_main_function(self): 18 | raise NotImplementedError() 19 | def define_for_block(self, size): 20 | raise NotImplementedError() 21 | def define_if_block(self, cond): 22 | raise NotImplementedError() 23 | def define_else_if_block(self, cond): 24 | raise NotImplementedError() 25 | def define_else_block(self, cond): 26 | raise NotImplementedError() 27 | # ADD 28 | def add_constant(self, name, type, value): 29 | raise NotImplementedError() 30 | def add_variable(self, name, type, default_value = None): 31 | raise NotImplementedError() 32 | def add_instruction(self, instr): 33 | raise NotImplementedError() 34 | # Others 35 | def param(self, name, type, default_value = None): 36 | raise NotImplementedError() 37 | def ref_type(self, type): 38 | raise NotImplementedError() 39 | def const_type(self, type): 40 | raise NotImplementedError() 41 | def list_type(self, type): 42 | raise NotImplementedError() 43 | def call(self, name, params): 44 | raise NotImplementedError() 45 | def assign(self, var, val): 46 | raise NotImplementedError() 47 | def matrix_get(self, name, i,j): 48 | raise NotImplementedError() 49 | def list_get(self, name, i): 50 | raise NotImplementedError() 51 | def nested(self, *names): 52 | raise NotImplementedError() 53 | -------------------------------------------------------------------------------- /src/generator/ik.py: -------------------------------------------------------------------------------- 1 | from ik_utils import * 2 | 3 | def add_matrix_typedef(gen): 4 | gen.add_instruction("template using matrix = {}".format( 5 | gen.matrix_type("r", "c") 6 | )) 7 | return gen 8 | 9 | def add_typed_variable_list(gen, vtype, vlist): 10 | for v in vlist: 11 | gen.add_variable(v[0], vtype, v[1]) 12 | return gen 13 | 14 | def add_fill_matrix_instructions(gen, name, matrix): 15 | rows = len(matrix) 16 | cols = len(matrix[0]) 17 | for row in range(0, rows): 18 | for col in range(0, cols): 19 | gen.add_instruction( 20 | gen.assign( 21 | gen.matrix_get( 22 | name, 23 | matrix_type(rows, cols), 24 | row, col 25 | ), 26 | str(matrix[row][col]) 27 | ) 28 | ) 29 | return gen 30 | 31 | def add_matrix_returning_function(gen, name, matrix, symbols): 32 | cse = get_cse(matrix) 33 | rows = len(matrix) 34 | cols = len(matrix[0]) 35 | func = gen.define_function( 36 | name, 37 | gen.void_type, 38 | map(lambda s: gen.param(str(s), gen.real_type), symbols) + 39 | [gen.param("matrix_out", gen.ref_type(matrix_type(rows, cols)))] 40 | ) 41 | add_typed_variable_list(func, func.real_type, cse[0]) 42 | add_fill_matrix_instructions(func, "matrix_out", cse[1]) 43 | return gen 44 | 45 | def add_forward_kinematics_function(gen, chain): 46 | ids = range(0, chain.get_num_params()) 47 | sym = list(map(lambda i: sympy.Symbol("q"+str(i)), ids)) 48 | mat = chain.forward_kinematics(sym) 49 | add_matrix_returning_function( 50 | gen, 51 | "forward_kinematics", 52 | mat, 53 | sym 54 | ) 55 | return gen 56 | 57 | def add_transposed_jacobian_function(gen, chain): 58 | ids = range(0, chain.get_num_params()) 59 | sym = list(map(lambda i: sympy.Symbol("q"+str(i)), ids)) 60 | mat = chain.forward_kinematics(sym) 61 | t_jacobian = get_transposed_jacobian(mat, sym) 62 | add_matrix_returning_function( 63 | gen, 64 | "transposed_jacobian", 65 | t_jacobian, 66 | sym 67 | ) 68 | return gen 69 | 70 | def add_chain_distance_instructions(gen, chain_name, ret_name, target_name, sym, rows, cols): 71 | gen.add_instruction( 72 | gen.call( 73 | "forward_kinematics", 74 | map(lambda s: str(s), sym) + [ret_name] 75 | ) 76 | ) 77 | for row in range(0, rows): 78 | for col in range(0, cols): 79 | gen.add_instruction( 80 | gen.assign( 81 | gen.matrix_get( 82 | ret_name, 83 | matrix_type(rows, cols), 84 | row, col 85 | ), 86 | " - ".join([ 87 | gen.matrix_get( 88 | target_name, 89 | matrix_type(rows, cols), 90 | row, col 91 | ), 92 | gen.matrix_get( 93 | ret_name, 94 | matrix_type(rows, cols), 95 | row, col 96 | ), 97 | ]) 98 | ) 99 | ) 100 | 101 | def add_norm_constant(gen, ret_name, dist_name, dist_rows, dist_cols): 102 | dist = get_flat_matrix( 103 | get_matrix_from_name(gen, dist_name, dist_rows, dist_cols) 104 | ) 105 | gen.add_constant( 106 | ret_name, 107 | gen.real_type, 108 | gen.call( 109 | "sqrt", 110 | [" + ".join(map(lambda e: "{e}*{e}".format(e=str(e)), dist))] 111 | ) 112 | ) 113 | return gen 114 | 115 | def add_ik_return_instructions(gen, sym, delta, delta_norm, dist_norm): 116 | delta = get_flat_matrix(get_matrix_from_name(gen, delta, len(sym), 1)) 117 | ifblock = gen.define_if_block("{} != 0".format(delta_norm)) 118 | ifblock.add_constant( 119 | "gain", 120 | ifblock.real_type, 121 | "{}*{}/{}".format("coeff", dist_norm, delta_norm) 122 | ) 123 | for (s,d) in zip(sym,delta): 124 | ifblock.add_instruction( 125 | ifblock.assign( 126 | s, 127 | "{}+({}*{})".format(s, d, "gain") 128 | ) 129 | ) 130 | 131 | def add_distance_from_target_function(gen, chain): 132 | ids = range(0, chain.get_num_params()) 133 | sym = list(map(lambda i: sympy.Symbol("q"+str(i)+"_io"), ids)) 134 | mat = chain.forward_kinematics(sym) 135 | shape = sympy.Matrix(mat).shape 136 | rows = shape[0] 137 | cols = shape[1] 138 | t_jacobian = get_transposed_jacobian(mat, sym) 139 | dist = get_matrix_from_name(gen, "dist", rows, cols) 140 | delta = get_matrix_mult(t_jacobian, get_column_matrix(dist)) 141 | assert(len(delta) == len(sym)) 142 | assert(len(delta[0]) == 1) 143 | func = gen.define_function( 144 | "distance_from_target", 145 | gen.real_type, 146 | [gen.param("target", gen.const_type(gen.ref_type(matrix_type(rows, cols))))] + 147 | map(lambda s: gen.param(str(s), gen.real_type), sym) + 148 | [gen.param("dist_out", gen.ref_type(matrix_type(rows, cols)))] 149 | ) 150 | add_chain_distance_instructions(func, chain.name, "dist_out", "target", sym, rows, cols) 151 | add_norm_constant(func, "ret", "dist_out", rows, cols) 152 | func.add_instruction("return ret") 153 | 154 | def add_inverse_kinematics_step_function(gen, chain): 155 | ids = range(0, chain.get_num_params()) 156 | sym = list(map(lambda i: sympy.Symbol("q"+str(i)+"_io"), ids)) 157 | mat = chain.forward_kinematics(sym) 158 | shape = sympy.Matrix(mat).shape 159 | rows = shape[0] 160 | cols = shape[1] 161 | t_jacobian = get_transposed_jacobian(mat, sym) 162 | dist = get_matrix_from_name(gen, "dist", rows, cols) 163 | delta = get_matrix_mult(t_jacobian, get_column_matrix(dist)) 164 | cse = get_cse(delta) 165 | assert(len(delta) == len(sym)) 166 | assert(len(delta[0]) == 1) 167 | func = gen.define_function( 168 | "inverse_kinematics_step", 169 | gen.void_type, 170 | [gen.param("target", gen.const_type(gen.ref_type(matrix_type(rows, cols))))] + 171 | map(lambda s: gen.param(str(s), gen.ref_type(gen.real_type)), sym) + 172 | [gen.param("coeff", gen.real_type)] 173 | ) 174 | func.add_variable("dist", matrix_type(rows, cols)) 175 | func.add_variable( 176 | "dist_norm", 177 | func.real_type, 178 | func.call( 179 | "distance_from_target", 180 | ["target"] + sym + ["dist"] 181 | ) 182 | ) 183 | func.add_variable("delta", matrix_type(len(sym),1)) 184 | add_typed_variable_list(func, func.real_type, cse[0]) 185 | add_fill_matrix_instructions(func, "delta", cse[1]) 186 | add_norm_constant(func, "delta_norm", "delta", len(sym), 1) 187 | add_ik_return_instructions(func, sym, "delta", "delta_norm", "dist_norm") 188 | 189 | def add_inverse_kinematics_function(gen, chain): 190 | ids = range(0, chain.get_num_params()) 191 | sym = list(map(lambda i: sympy.Symbol("q"+str(i)+"_io"), ids)) 192 | mat = chain.forward_kinematics(sym) 193 | shape = sympy.Matrix(mat).shape 194 | rows = shape[0] 195 | cols = shape[1] 196 | func = gen.define_function( 197 | "inverse_kinematics", 198 | "bool", 199 | [gen.param("target", gen.const_type(gen.ref_type(matrix_type(rows, cols))))] + 200 | map(lambda s: gen.param(str(s), gen.ref_type(gen.real_type)), sym) + 201 | [ 202 | gen.param("coeff", gen.real_type), 203 | gen.param("stop_dist", gen.real_type), 204 | gen.param("max_iter", gen.integer_type), 205 | ]) 206 | func.add_variable("tmp", matrix_type(rows, cols)) 207 | ifblock = func.define_if_block( 208 | "stop_dist >= " + 209 | func.call( 210 | "distance_from_target", 211 | ["target"] + sym + ["tmp"] 212 | ) 213 | ) 214 | ifblock.add_instruction("return true") 215 | forblock = func.define_for_block("max_iter") 216 | forblock.add_instruction( 217 | func.call( 218 | "inverse_kinematics_step", 219 | ["target"] + sym + ["coeff"] 220 | ) 221 | ) 222 | ifblock = forblock.define_if_block( 223 | "stop_dist >= " + 224 | func.call( 225 | "distance_from_target", 226 | ["target"] + sym + ["tmp"] 227 | ) 228 | ) 229 | ifblock.add_instruction("return true") 230 | func.add_instruction("return false") 231 | 232 | 233 | def add_ik_module(gen, chain): 234 | mod = gen.define_module(chain.name) 235 | add_matrix_typedef(mod) 236 | add_forward_kinematics_function(mod, chain) 237 | add_distance_from_target_function(mod, chain) 238 | add_inverse_kinematics_step_function(mod, chain) 239 | add_inverse_kinematics_function(mod, chain) 240 | return gen 241 | -------------------------------------------------------------------------------- /src/generator/ik_utils.py: -------------------------------------------------------------------------------- 1 | from cpp import * 2 | 3 | import sympy 4 | import numpy 5 | 6 | class IkMatrixType: 7 | def __init__(self, rows, cols): 8 | self.rows = rows 9 | self.cols = cols 10 | def __str__(self): 11 | return "matrix<{r}, {c}>".format( 12 | r=self.rows, 13 | c=self.cols 14 | ) 15 | 16 | def get_cse(matrix): 17 | rows = len(matrix) 18 | cols = len(matrix[0]) 19 | expr_list = [] 20 | coord_list = {} 21 | for row in range(0, rows): 22 | for col in range(0, cols): 23 | scase = sympy.sympify(matrix[row][col]) 24 | coord_list[(row,col)] = len(expr_list) 25 | expr_list.append(scase) 26 | cse = sympy.cse(expr_list, sympy.numbered_symbols('tmp')) 27 | tmp_list = cse[0] 28 | expr_list = cse[1] 29 | expr_mat = [] 30 | for row in range(0, rows): 31 | row_list = [] 32 | for col in range(0, cols): 33 | i = coord_list[(row,col)] 34 | e = expr_list[i] 35 | row_list.append(expr_list[i]) 36 | expr_mat.append(row_list) 37 | return (tmp_list, expr_mat) 38 | 39 | def get_transposed_jacobian(mat, sym): 40 | flat = [] 41 | for row in mat: 42 | for case in row: 43 | flat.append(case) 44 | t_jacobian = [] 45 | for s in sym: 46 | t_jacobian.append(list(map(lambda e: e.diff(s), flat))) 47 | return t_jacobian 48 | 49 | def matrix_type(r, c): 50 | return IkMatrixType(r,c) 51 | 52 | def get_chain_symbols(chain): 53 | ids = range(0, chain.get_num_params()) 54 | pnames = list(map(lambda i: "q"+str(i)+"_io", ids)) 55 | return list(map(lambda i: sympy.Symbol(i), pnames)) 56 | 57 | def get_matrix_mult(m1, m2): 58 | ret = sympy.Matrix(m1) * sympy.Matrix(m2) 59 | return list(map(lambda e: [e], list(ret))) 60 | 61 | def get_matrix_from_name(gen, name, rows, cols): 62 | ret = [] 63 | for row in range(0, rows): 64 | ret_row = [] 65 | for col in range(0, cols): 66 | ret_row.append( 67 | sympy.Symbol(gen.matrix_get(name, matrix_type(rows, cols), row, col)) 68 | ) 69 | ret.append(ret_row) 70 | return ret 71 | 72 | def get_column_matrix(matrix): 73 | ret = [] 74 | rows = len(matrix) 75 | cols = len(matrix[0]) 76 | for row in range(0, rows): 77 | for col in range(0, cols): 78 | ret.append([matrix[row][col]]) 79 | return ret 80 | 81 | def get_flat_matrix(matrix): 82 | ret = [] 83 | rows = len(matrix) 84 | cols = len(matrix[0]) 85 | for row in range(0, rows): 86 | for col in range(0, cols): 87 | ret.append(matrix[row][col]) 88 | return ret 89 | 90 | -------------------------------------------------------------------------------- /src/kinematics/__init__.py: -------------------------------------------------------------------------------- 1 | from chain import * 2 | from matrix_chain_element import * 3 | -------------------------------------------------------------------------------- /src/kinematics/chain.py: -------------------------------------------------------------------------------- 1 | """ 2 | Copyright (c) 2015, Xenomorphales 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | * Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | * Neither the name of Aversive++ nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | """ 30 | 31 | from chain_element import ChainElement 32 | import numpy as np 33 | 34 | class Chain(ChainElement): 35 | def __init__(self, name, elements): 36 | ChainElement.__init__(self, name) 37 | self.elements = elements 38 | 39 | def get_num_params(self): 40 | _to_num_params = lambda e: e.get_num_params() 41 | return sum(list(map(_to_num_params, self.elements))) 42 | 43 | def get_matrix(self, *params): 44 | if len(params) != self.get_num_params(): 45 | return None 46 | first = self.elements[0] 47 | ret = first.get_matrix(*params[:first.get_num_params()]) 48 | params = params[first.get_num_params():] 49 | for e in self.elements[1:]: 50 | mat = np.asarray(e.get_matrix(*params[:e.get_num_params()])) 51 | ret = np.dot(ret, mat) 52 | params = params[e.get_num_params():] 53 | return ret 54 | 55 | def forward_kinematics(self, params): 56 | return self.get_matrix(*params) 57 | -------------------------------------------------------------------------------- /src/kinematics/chain_element.py: -------------------------------------------------------------------------------- 1 | """ 2 | Copyright (c) 2015, Xenomorphales 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | * Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | * Neither the name of Aversive++ nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | """ 30 | 31 | class ChainElement: 32 | def __init__(self, name): 33 | self.name = name 34 | 35 | def get_num_params(self): 36 | return 0 37 | 38 | def get_matrix(self): 39 | return None 40 | -------------------------------------------------------------------------------- /src/kinematics/matrix_chain_element.py: -------------------------------------------------------------------------------- 1 | """ 2 | Copyright (c) 2015, Xenomorphales 3 | All rights reserved. 4 | 5 | Redistribution and use in source and binary forms, with or without 6 | modification, are permitted provided that the following conditions are met: 7 | 8 | * Redistributions of source code must retain the above copyright notice, this 9 | list of conditions and the following disclaimer. 10 | 11 | * Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | 15 | * Neither the name of Aversive++ nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 22 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 23 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 24 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 25 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 27 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 28 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | """ 30 | 31 | from chain_element import ChainElement 32 | import numpy as np 33 | import sympy 34 | 35 | class ConstantMatrixChainElement(ChainElement): 36 | def __init__(self, name, parent, matrix): 37 | ChainElement.__init__(self, name) 38 | if type(matrix) == np.matrix: 39 | self.matrix = matrix 40 | else: 41 | self.matrix = np.matrix(matrix) 42 | 43 | def get_matrix(self): 44 | return np.asarray(self.matrix) 45 | 46 | class VariableMatrixChainElement(ChainElement): 47 | def __init__(self, name, parent, matrix, symbols): 48 | ChainElement.__init__(self, name) 49 | self.symbols = symbols 50 | self.matrix = matrix 51 | self.lambda_matrix = sympy.lambdify(self.symbols, sympy.Matrix(self.matrix), "numpy") 52 | self.sympy_lambda_matrix = sympy.lambdify(self.symbols, sympy.Matrix(self.matrix), "sympy") 53 | 54 | def get_num_params(self): 55 | return len(self.symbols) 56 | 57 | def get_matrix(self, *args): 58 | matrix = self.lambda_matrix 59 | if any(list(map(lambda e: type(e) == sympy.Symbol, args))): 60 | matrix = self.sympy_lambda_matrix 61 | if len(self.symbols) == len(args): 62 | return np.asarray(matrix(*args)) 63 | else: 64 | raise IndexError("{} parameters for {} symbols".format(len(args), len(self.symbols))) 65 | -------------------------------------------------------------------------------- /src/main.py: -------------------------------------------------------------------------------- 1 | from kinematics import * 2 | from generator import * 3 | import blender_utils 4 | import sympy 5 | import sys 6 | 7 | def main(argv): 8 | if len(argv) < 2: 9 | print("usage : " + argv[0] + " []") 10 | exit() 11 | 12 | x = sympy.Symbol("x") 13 | y = sympy.Symbol("y") 14 | z = sympy.Symbol("z") 15 | 16 | blender_utils.call_blender_export(argv[1]) 17 | 18 | if len(argv) == 2: 19 | l = blender_utils.read_json("blender.out.json") 20 | for e in l: 21 | print(e["name"]) 22 | elif len(argv) == 3: 23 | endpoint = argv[2].replace("/", "_") 24 | chain = blender_utils.extract_chain( 25 | blender_utils.read_json("blender.out.json"), 26 | argv[2] 27 | ) 28 | chain.name = chain.name.replace("/", "_") 29 | gen = CppProjectGenerator() 30 | add_ik_module(gen, chain) 31 | for fname in gen: 32 | with open(fname, "w+") as f: 33 | f.write(str(gen[fname])) 34 | 35 | -------------------------------------------------------------------------------- /src/test.py: -------------------------------------------------------------------------------- 1 | from kinematics import * 2 | from generator import * 3 | import numpy as np 4 | import sympy 5 | 6 | x = sympy.Symbol("x") 7 | y = sympy.Symbol("y") 8 | z = sympy.Symbol("z") 9 | 10 | e1 = ConstantMatrixChainElement("e1", None, 11 | [[0,1,0],[1,0,0],[0,0,1]]) 12 | 13 | e2 = VariableMatrixChainElement("e1", None, 14 | [[x,0,0],[0,y,0],[0,0,1]], [x,y]) 15 | 16 | chain = Chain("my_chain", [e1, e2]) 17 | 18 | gen = CppProjectGenerator() 19 | add_ik_module(gen, chain) 20 | print(gen) 21 | --------------------------------------------------------------------------------