├── fabrik_full_body ├── __init__.py ├── angle_calculator │ ├── __init__.py │ ├── body_part_numbering.py │ ├── angle_calculator.py │ ├── lower_arm.py │ ├── trunck.py │ ├── neck.py │ ├── leg.py │ ├── wrist.py │ └── upper_arm.py ├── body_number.jpg ├── singleton.py ├── output_writer.py ├── util.py ├── input_reader.py ├── draw.py ├── constraints.py └── fabrik.py ├── test ├── inputs │ ├── target.txt │ ├── bone_twist_constraints.txt │ ├── constraint_type.txt │ ├── joints_constraint.txt │ ├── joints_position.txt │ ├── joints_position_fixed.txt │ └── orientation.txt ├── test_fabrik.py └── outputs │ └── angles.txt ├── .idea ├── .gitignore ├── vcs.xml ├── misc.xml ├── inspectionProfiles │ └── profiles_settings.xml ├── modules.xml └── FABRIK.iml ├── Mat.py ├── setup.py ├── LICENSE ├── .gitignore ├── README.md └── h.py /fabrik_full_body/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /fabrik_full_body/angle_calculator/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /test/inputs/target.txt: -------------------------------------------------------------------------------- 1 | 0.45, 0.4, 0.9 2 | 0.71, 0, 0, 0.71 -------------------------------------------------------------------------------- /.idea/.gitignore: -------------------------------------------------------------------------------- 1 | 2 | # Default ignored files 3 | /workspace.xml -------------------------------------------------------------------------------- /fabrik_full_body/body_number.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Atiehmerikh/FABRIK_Full_Body/HEAD/fabrik_full_body/body_number.jpg -------------------------------------------------------------------------------- /test/inputs/bone_twist_constraints.txt: -------------------------------------------------------------------------------- 1 | 40 2 | 40 3 | 40 4 | 40 5 | 40 6 | 40 7 | 40 8 | 40 9 | 40 10 | 40 11 | 40 12 | 40 13 | 40 14 | 40 15 | 40 16 | 40 17 | 40 18 | 40 19 | 40 -------------------------------------------------------------------------------- /.idea/vcs.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /.idea/misc.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /.idea/inspectionProfiles/profiles_settings.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | -------------------------------------------------------------------------------- /test/inputs/constraint_type.txt: -------------------------------------------------------------------------------- 1 | BALL 2 | BALL 3 | hinge 4 | BALL 5 | endEffector 6 | BALL 7 | hinge 8 | BALL 9 | endEffector 10 | BALL 11 | BALL 12 | hinge 13 | BALL 14 | endEffector 15 | hinge 16 | BALL 17 | endEffector 18 | BALL 19 | endEffector -------------------------------------------------------------------------------- /fabrik_full_body/singleton.py: -------------------------------------------------------------------------------- 1 | class Singleton(type): 2 | _instances = {} 3 | def __call__(cls, *args, **kwargs): 4 | if cls not in cls._instances: 5 | cls._instances[cls] = super(Singleton, cls).__call__(*args, **kwargs) 6 | return cls._instances[cls] -------------------------------------------------------------------------------- /.idea/modules.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /test/inputs/joints_constraint.txt: -------------------------------------------------------------------------------- 1 | 80 80 80 80 2 | 80 80 80 80 3 | 0 80 0 0 4 | 80 80 80 80 5 | 80 80 80 80 6 | 80 80 80 80 7 | 0 80 0 0 8 | 80 80 80 80 9 | 80 80 80 80 10 | 80 80 80 80 11 | 80 80 80 80 12 | 0 0 0 80 13 | 80 80 80 80 14 | 80 80 80 80 15 | 0 0 0 80 16 | 80 80 80 80 17 | 80 80 80 80 18 | 80 80 80 80 19 | 80 80 80 80 -------------------------------------------------------------------------------- /test/inputs/joints_position.txt: -------------------------------------------------------------------------------- 1 | 0 -0.12 0.12 2 | -0.25 -0.39 0.39 3 | -0.32 -0.22 0.2 4 | -0.24 0.01 0.2 5 | -0.23 0.1 0.2 6 | 0.25 -0.39 0.39 7 | 0.32 -0.22 0.2 8 | 0.24 0.01 0.2 9 | 0.23 0.1 0.2 10 | -0.16 0 0 11 | 0.16 0 0 12 | -0.16 0.51 0 13 | -0.16 0.9 -0.33 14 | -0.16 1.12 -0.33 15 | 0.16 0.51 0 16 | 0.16 0.9 -0.33 17 | 0.16 1.12 -0.33 18 | 0 -0.39 0.39 19 | 0 -0.56 0.56 -------------------------------------------------------------------------------- /test/inputs/joints_position_fixed.txt: -------------------------------------------------------------------------------- 1 | 0 -0.12 0.12 2 | -0.25 -0.39 0.39 3 | -0.32 -0.22 0.2 4 | -0.24 0.01 0.2 5 | -0.23 0.1 0.2 6 | 0.25 -0.39 0.39 7 | 0.32 -0.22 0.2 8 | 0.24 0.01 0.2 9 | 0.23 0.1 0.2 10 | -0.16 0 0 11 | 0.16 0 0 12 | -0.16 0.51 0 13 | -0.16 0.9 -0.33 14 | -0.16 1.12 -0.33 15 | 0.16 0.51 0 16 | 0.16 0.9 -0.33 17 | 0.16 1.12 -0.33 18 | 0 -0.39 0.39 19 | 0 -0.56 0.56 -------------------------------------------------------------------------------- /test/inputs/orientation.txt: -------------------------------------------------------------------------------- 1 | 0.71 0 0 0.71 2 | 0.71 0 0 0.71 3 | 0.71 0 0 0.71 4 | 0.71 0 0 0.71 5 | 0.71 0 0 0.71 6 | 0.71 0 0 0.71 7 | 0.71 0 0 0.71 8 | 0.71 0 0 0.71 9 | 0.71 0 0 0.71 10 | 0.71 0 0 0.71 11 | 0.71 0 0 0.71 12 | 0.71 0 0 0.71 13 | 0.71 0 0 0.71 14 | 0.71 0 0 0.71 15 | 0.71 0 0 0.71 16 | 0.71 0 0 0.71 17 | 0.71 0 0 0.71 18 | 0.71 0 0 0.71 19 | 0.71 0 0 0.71 -------------------------------------------------------------------------------- /fabrik_full_body/output_writer.py: -------------------------------------------------------------------------------- 1 | from fabrik_full_body.singleton import Singleton 2 | class OutputWriter(metaclass=Singleton): 3 | def __init__(self, base_address="./outputs/", angles_file_address = "angles.txt"): 4 | self.base_address = base_address 5 | self.angles_file_address = base_address + angles_file_address 6 | 7 | def angle_writer(self): 8 | return open(self.angles_file_address, "w") 9 | -------------------------------------------------------------------------------- /Mat.py: -------------------------------------------------------------------------------- 1 | def multiply_two_quaternion(q1, q2): 2 | a = q1[0] 3 | b = q1[1] 4 | c = q1[2] 5 | d = q1[3] 6 | e = q2[0] 7 | f = q2[1] 8 | g = q2[2] 9 | h = q2[3] 10 | 11 | m0 = round(a * e - b * f - c * g - d * h, 2) 12 | m1 = round(b * e + a * f + c * h - d * g, 2) 13 | m2 = round(a * g - b * h + c * e + d * f, 2) 14 | m3 = round(a * h + b * g - c * f + d * e, 2) 15 | return [m0, m1, m2, m3] -------------------------------------------------------------------------------- /.idea/FABRIK.iml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | -------------------------------------------------------------------------------- /test/test_fabrik.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append('..') 3 | from fabrik_full_body import fabrik, input_reader 4 | 5 | 6 | 7 | def main(): 8 | reader = input_reader.InputReader() 9 | manipulator = fabrik.FABRIK(reader.joints(), 10 | reader.initial_joints_position(), 11 | reader.orientation(), 12 | reader.target_position(), 13 | reader.target_orientation(), 14 | reader.joints_constraints(), 15 | reader.constraint_type(), 16 | reader.bone_orientation_limit()) 17 | 18 | manipulator.solve() 19 | 20 | 21 | if __name__ == "__main__": 22 | main() -------------------------------------------------------------------------------- /fabrik_full_body/angle_calculator/body_part_numbering.py: -------------------------------------------------------------------------------- 1 | class BodyPartNumber: 2 | def neck(self): 3 | return [17, 18] 4 | 5 | def trunk_upper_body(self): 6 | return [0, 1, 17, 5] 7 | 8 | def right_arm(self): 9 | return [1, 2, 3, 4] 10 | 11 | def left_arm(self): 12 | return [5, 6, 7, 8] 13 | 14 | def trunk_lower_body(self): 15 | return [0, 9, 19, 10] 16 | 17 | def trunk_whole_body(self): 18 | return [9, 10, 17, 0] 19 | 20 | def right_leg(self): 21 | return [9, 11, 12, 13] 22 | 23 | def left_leg(self): 24 | return [10, 14, 15, 16] 25 | 26 | def right_shoulder(self): 27 | return [17,1] 28 | 29 | def left_shoulder(self): 30 | return [17,5] -------------------------------------------------------------------------------- /fabrik_full_body/util.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import Mat as Mat 4 | 5 | 6 | # Finding the distance between two arbitrary point 7 | def distance(i, j): 8 | return math.sqrt((i[0] - j[0]) ** 2 + (i[1] - j[1]) ** 2 + (i[2] - j[2]) ** 2) 9 | 10 | def find_rotation_quaternion(outer_quaternion, inner_quaternion): 11 | conjucate = [outer_quaternion[0], -outer_quaternion[1], -outer_quaternion[2], -outer_quaternion[3]] 12 | length = math.sqrt(outer_quaternion[0] ** 2 + outer_quaternion[1] ** 2 + 13 | outer_quaternion[2] ** 2 + outer_quaternion[3] ** 2) 14 | inverse = np.dot(conjucate, (1 / length)) 15 | rotation = Mat.multiply_two_quaternion(inner_quaternion, inverse) 16 | return rotation 17 | -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import setuptools 2 | 3 | with open("README.md", "r") as fh: 4 | long_description = fh.read() 5 | 6 | setuptools.setup( 7 | name="fabrik_full_body", 8 | version="1.3.0", 9 | author="Example Author", 10 | author_email="atieh.merikh.nejadasl@vub.be", 11 | description="It is an implementation of the FABRIK‌ method", 12 | long_description=long_description, 13 | long_description_content_type="text/markdown", 14 | url="https://github.com/Atiehmerikh/FABRIK_python", 15 | packages=setuptools.find_packages(), 16 | classifiers=[ 17 | "Programming Language :: Python :: 3", 18 | "License :: OSI Approved :: MIT License", 19 | "Operating System :: OS Independent", 20 | ], 21 | python_requires='>=3.6', 22 | ) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2020 Atiehmerikh 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /test/outputs/angles.txt: -------------------------------------------------------------------------------- 1 | Neck bending 2 | -34.04669207500908 3 | Neck side bending 4 | 61.28168912784212 5 | Neck twist 6 | 00.0 7 | Trunk bending 8 | 70.48655847519758 9 | Trunk side bending 10 | 3.2507073500279136 11 | Trunk twist 12 | 0 13 | Upper right leg flexion 14 | 39.3366393613926 15 | Upper left leg flexion 16 | 49.593184129762015 17 | Upper right leg abduction 18 | 0 19 | Upper left leg abduction 20 | 0 21 | Lower right leg flexion 22 | 0 23 | Lower left leg flexion 24 | 0 25 | Right foot flexion 26 | 0 27 | Left foot flexion 28 | 0 29 | Right foot bending 30 | 0 31 | Left foot bending 32 | 0 33 | Right foot twist 34 | 0 35 | Left foot twist 36 | 0 37 | Upper right leg rotation 38 | 0 39 | Upper left leg rotation 40 | 0 41 | Upper right arm flexion 42 | 42.90984658644293 43 | Upper left arm flexion 44 | 40.539393704739965 45 | Upper right arm abduction 46 | -0.5071694735585738 47 | Upper left arm abduction 48 | -0.4760364561386654 49 | Upper right arm raise 50 | 90.26213610149618 51 | Upper left arm raise 52 | 89.73786389850382 53 | Upper right arm rotation 54 | 0 55 | Upper left arm rotation 56 | 0 57 | Lower right arm flexion 58 | 1381.698502296894 59 | Lower left arm flexion 60 | 1345.0868574882966 61 | Right hand flexion 62 | 6.425651057759326 63 | Left hand flexion 64 | 14.97445177198807 65 | Right hand bending 66 | -4.758978768350005 67 | Left hand bending 68 | 10.993697986873798 69 | Right hand twist 70 | 0.0 71 | Left hand twist 72 | 0.0 73 | -------------------------------------------------------------------------------- /fabrik_full_body/angle_calculator/angle_calculator.py: -------------------------------------------------------------------------------- 1 | import fabrik_full_body.angle_calculator.neck as neck 2 | import fabrik_full_body.angle_calculator.trunck as trunk 3 | import fabrik_full_body.angle_calculator.leg as leg 4 | import fabrik_full_body.angle_calculator.upper_arm as upperArm 5 | import fabrik_full_body.angle_calculator.lower_arm as lowerArm 6 | import fabrik_full_body.angle_calculator.wrist as wrist 7 | 8 | 9 | class AngleCalculator: 10 | def __init__(self,file,joints,orientation): 11 | self.file = file 12 | self.joints = joints 13 | self.orientation = orientation 14 | 15 | def calculate(self): 16 | m_neck = neck.Neck(self.joints,self.orientation,self.file) 17 | m_neck.neck_flex_calculator() 18 | m_neck.neck_side_calculator() 19 | m_neck.neck_torsion_calculator() 20 | 21 | m_trunk = trunk.Trunk(self.joints,self.orientation,self.file) 22 | m_trunk.trunk_flex_calculator() 23 | m_trunk.trunk_side_calculator() 24 | m_trunk.trunk_torsion_calculator() 25 | 26 | m_leg = leg.Leg(self.joints, self.file) 27 | m_leg.leg_degree() 28 | 29 | m_UA = upperArm.UpperArm(self.joints, self.file) 30 | m_UA.upper_arm_flex() 31 | m_UA.upper_arm_side_bending() 32 | m_UA.shoulder_rise() 33 | m_UA.upper_arm_rotation() 34 | 35 | m_LA = lowerArm.LowerArm(self.joints, self.file) 36 | m_LA.lower_arm_degree() 37 | 38 | m_wrist = wrist.Wrist(self.joints,self.orientation,self.file) 39 | m_wrist.wrist_flex() 40 | m_wrist.wrist_side() 41 | m_wrist.wrist_torsion() -------------------------------------------------------------------------------- /fabrik_full_body/angle_calculator/lower_arm.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import fabrik_full_body.angle_calculator.body_part_numbering as bodyNum 4 | 5 | 6 | class LowerArm: 7 | def __init__(self, joints,file): 8 | self.file = file 9 | self.joints = joints 10 | 11 | def lower_arm_degree(self): 12 | m_body_number = bodyNum.BodyPartNumber() 13 | right_arm_joint_numbers = m_body_number.right_arm() 14 | left_arm_joint_numbers = m_body_number.left_arm() 15 | 16 | right_shoulder_elbow_vector = self.joints[right_arm_joint_numbers[1]] - self.joints[right_arm_joint_numbers[0]] 17 | left_shoulder_elbow_vector = self.joints[left_arm_joint_numbers[1]] - self.joints[left_arm_joint_numbers[0]] 18 | 19 | right_elbow_wrist_vector = self.joints[right_arm_joint_numbers[2]] - self.joints[right_arm_joint_numbers[1]] 20 | left_elbow_wrist_vector = self.joints[left_arm_joint_numbers[2]] - self.joints[left_arm_joint_numbers[1]] 21 | 22 | # right and left arm degree in saggital plane 23 | right_degree = math.degrees(math.acos(np.dot(right_shoulder_elbow_vector, right_elbow_wrist_vector)) / ( 24 | (math.sqrt(np.dot(right_shoulder_elbow_vector, right_shoulder_elbow_vector))) * ( 25 | math.sqrt(np.dot(right_elbow_wrist_vector, right_elbow_wrist_vector))))) 26 | left_degree = math.degrees(math.acos(np.dot(left_shoulder_elbow_vector, left_elbow_wrist_vector)) / ( 27 | (math.sqrt(np.dot(left_shoulder_elbow_vector, left_shoulder_elbow_vector))) * ( 28 | math.sqrt(np.dot(left_elbow_wrist_vector, left_elbow_wrist_vector))))) 29 | 30 | self.file.write("Lower right arm flexion \n") 31 | self.file.write(str(right_degree)) 32 | self.file.write("\n") 33 | 34 | self.file.write("Lower left arm flexion \n") 35 | self.file.write(str(left_degree)) 36 | self.file.write("\n") 37 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | pip-wheel-metadata/ 24 | share/python-wheels/ 25 | *.egg-info/ 26 | .installed.cfg 27 | *.egg 28 | MANIFEST 29 | 30 | # PyInstaller 31 | # Usually these files are written by a python script from a template 32 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 33 | *.manifest 34 | *.spec 35 | 36 | # Installer logs 37 | pip-log.txt 38 | pip-delete-this-directory.txt 39 | 40 | # Unit test / coverage reports 41 | htmlcov/ 42 | .tox/ 43 | .nox/ 44 | .coverage 45 | .coverage.* 46 | .cache 47 | nosetests.xml 48 | coverage.xml 49 | *.cover 50 | *.py,cover 51 | .hypothesis/ 52 | .pytest_cache/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | target/ 76 | 77 | # Jupyter Notebook 78 | .ipynb_checkpoints 79 | 80 | # IPython 81 | profile_default/ 82 | ipython_config.py 83 | 84 | # pyenv 85 | .python-version 86 | 87 | # pipenv 88 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 89 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 90 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 91 | # install all needed dependencies. 92 | #Pipfile.lock 93 | 94 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow 95 | __pypackages__/ 96 | 97 | # Celery stuff 98 | celerybeat-schedule 99 | celerybeat.pid 100 | 101 | # SageMath parsed files 102 | *.sage.py 103 | 104 | # Environments 105 | .env 106 | .venv 107 | env/ 108 | venv/ 109 | ENV/ 110 | env.bak/ 111 | venv.bak/ 112 | 113 | # Spyder project settings 114 | .spyderproject 115 | .spyproject 116 | 117 | # Rope project settings 118 | .ropeproject 119 | 120 | # mkdocs documentation 121 | /site 122 | 123 | # mypy 124 | .mypy_cache/ 125 | .dmypy.json 126 | dmypy.json 127 | 128 | # Pyre type checker 129 | .pyre/ -------------------------------------------------------------------------------- /fabrik_full_body/input_reader.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from fabrik_full_body.singleton import Singleton 3 | 4 | class InputReader(metaclass=Singleton): 5 | def __init__(self, base_address = "./inputs/", 6 | joints_position = "joints_position.txt", 7 | initial_joints_position = "joints_position_fixed.txt", 8 | orientation = "orientation.txt", 9 | target = "target.txt", 10 | joints_constraints = "joints_constraint.txt", 11 | constraint_type = "constraint_type.txt", 12 | bone_orientation_limit = "bone_twist_constraints.txt"): 13 | self.base_address = base_address 14 | self.joints_position_file_address = self.base_address + joints_position 15 | self.initial_joints_position_file_address = self.base_address + initial_joints_position 16 | self.orientation_file_address = self.base_address + orientation 17 | self.target_file_address = self.base_address + target 18 | self.joints_constraints_file_address = self.base_address + joints_constraints 19 | self.constraint_type_file_address = self.base_address + constraint_type 20 | self.bone_orientation_limit_file_address = self.base_address + bone_orientation_limit 21 | self.__clone_joints_position() 22 | 23 | def joints(self): 24 | return np.loadtxt(self.joints_position_file_address) 25 | 26 | def initial_joints_position(self): 27 | return np.loadtxt(self.initial_joints_position_file_address) 28 | 29 | def orientation(self): 30 | return np.loadtxt(self.orientation_file_address) 31 | 32 | def target_position(self): 33 | with open(self.target_file_address) as f: 34 | l = f.readline() 35 | return [float(i) for i in l.split(',')] 36 | 37 | def target_orientation(self): 38 | with open(self.target_file_address) as f: 39 | l = f.readline() 40 | l = f.readline() 41 | return [float(i) for i in l.split(',')] 42 | 43 | def joints_constraints(self): 44 | return np.loadtxt(self.joints_constraints_file_address) 45 | 46 | def constraint_type(self): 47 | with open(self.constraint_type_file_address) as f: 48 | return [line.rstrip() for line in f] 49 | 50 | def bone_orientation_limit(self): 51 | return np.loadtxt(self.bone_orientation_limit_file_address) 52 | 53 | def __clone_joints_position(self): 54 | with open(self.joints_position_file_address) as f: 55 | with open(self.initial_joints_position_file_address, "w") as f1: 56 | for line in f: 57 | f1.write(line) -------------------------------------------------------------------------------- /fabrik_full_body/angle_calculator/trunck.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import fabrik_full_body.util as util 4 | import fabrik_full_body.angle_calculator.body_part_numbering as bodyNum 5 | 6 | 7 | class Trunk: 8 | def __init__(self, joints,orientation,file): 9 | self.joints = joints 10 | self.orientation = orientation 11 | self.file = file 12 | 13 | def trunk_plane(self): 14 | m_body_number = bodyNum.BodyPartNumber() 15 | trunk_joint_numbers = m_body_number.trunk_whole_body() 16 | 17 | # finding a plane of upper body 18 | 19 | u = self.joints[trunk_joint_numbers[2]] - self.joints[trunk_joint_numbers[1]] 20 | v = self.joints[trunk_joint_numbers[2]] - self.joints[trunk_joint_numbers[0]] 21 | 22 | normal_plane = np.cross(u, v) 23 | return normal_plane 24 | 25 | def trunk_flex_calculator(self): 26 | normal_plane = self.trunk_plane() 27 | z_vector = np.array([0, 0, 1]) 28 | 29 | trunk_flex = math.degrees(math.acos(np.dot(z_vector, normal_plane) / ( 30 | math.sqrt(np.dot(normal_plane, normal_plane)) * math.sqrt(np.dot(z_vector, z_vector))))) 31 | 32 | self.file.write("Trunk bending \n") 33 | self.file.write(str(trunk_flex)) 34 | self.file.write("\n") 35 | 36 | def trunk_side_calculator(self): 37 | m_body_number = bodyNum.BodyPartNumber() 38 | trunk_joint_numbers = m_body_number.trunk_whole_body() 39 | 40 | normal_plane_xz = np.array([0, 1, 0]) 41 | z_vector = np.array([0, 0, 1]) 42 | spine_vector = self.joints[trunk_joint_numbers[2]] - self.joints[trunk_joint_numbers[3]] 43 | 44 | project_spine_on_xz_plane = spine_vector - np.dot(spine_vector, normal_plane_xz) * normal_plane_xz 45 | 46 | trunk_side_bending = math.degrees(math.acos(np.dot(project_spine_on_xz_plane, z_vector) / ( 47 | math.sqrt(np.dot(project_spine_on_xz_plane, project_spine_on_xz_plane)) * math.sqrt( 48 | np.dot(z_vector, z_vector))))) 49 | 50 | self.file.write("Trunk side bending \n") 51 | self.file.write(str(trunk_side_bending)) 52 | self.file.write("\n") 53 | 54 | def trunk_torsion_calculator(self): 55 | # In here the rotor needed to transfer orientation frame of core joint to neck joint is calculated 56 | # this considered as twist 57 | m_body_number = bodyNum.BodyPartNumber() 58 | trunk_joint_numbers = m_body_number.trunk_whole_body() 59 | q1 = self.orientation[trunk_joint_numbers[2]] # neck 60 | q2 = self.orientation[trunk_joint_numbers[3]] # core 61 | # finding the rotor that express rotation between two orientational frame(between outer and inner joint) 62 | rotor = util.find_rotation_quaternion(q1, q2) 63 | trunk_twist = math.acos(rotor[0]) * 2 * (180 / np.pi) 64 | self.file.write("Trunk twist \n") 65 | self.file.write(str(0)) 66 | self.file.write("\n") 67 | return trunk_twist 68 | -------------------------------------------------------------------------------- /fabrik_full_body/angle_calculator/neck.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import fabrik_full_body.angle_calculator.body_part_numbering as bodyNum 4 | import fabrik_full_body.util as util 5 | 6 | 7 | class Neck: 8 | def __init__(self, joints,orientations,file): 9 | self.joints = joints 10 | self.file = file 11 | self.orientation = orientations 12 | 13 | def trunk_plane(self): 14 | m_body_number = bodyNum.BodyPartNumber() 15 | trunk_joint_numbers = m_body_number.trunk_upper_body() 16 | 17 | # finding a plane of upper body 18 | u = self.joints[trunk_joint_numbers[1]] - self.joints[trunk_joint_numbers[0]] 19 | v = self.joints[trunk_joint_numbers[3]] - self.joints[trunk_joint_numbers[0]] 20 | 21 | normal_plane = np.cross(u, v) 22 | return normal_plane 23 | 24 | def neck_flex_calculator(self): 25 | m_body_number = bodyNum.BodyPartNumber() 26 | neck_joint_numbers = m_body_number.neck() 27 | normal_plane = self.trunk_plane() 28 | neck_vector = self.joints[neck_joint_numbers[1]] - self.joints[neck_joint_numbers[0]] 29 | 30 | neck_flex = 90 - math.degrees(math.acos(np.dot(neck_vector, normal_plane) / ( 31 | math.sqrt(np.dot(normal_plane, normal_plane)) * math.sqrt(np.dot(neck_vector, neck_vector))))) 32 | 33 | self.file.write("Neck bending \n") 34 | self.file.write(str(neck_flex)) 35 | self.file.write("\n") 36 | 37 | def neck_side_calculator(self): 38 | m_body_number = bodyNum.BodyPartNumber() 39 | neck_joint_numbers = m_body_number.neck() 40 | trunk_joint_numbers = m_body_number.trunk_upper_body() 41 | 42 | normal_plane = self.trunk_plane() 43 | neck_vector = self.joints[neck_joint_numbers[1]] - self.joints[neck_joint_numbers[0]] 44 | project_neck_on_trunk_plane = neck_vector - np.dot(neck_vector, normal_plane) * normal_plane 45 | 46 | spine_vector = self.joints[trunk_joint_numbers[2]] - self.joints[trunk_joint_numbers[0]] 47 | 48 | neck_side_bending = math.degrees(math.acos(np.dot(project_neck_on_trunk_plane, spine_vector) / ( 49 | math.sqrt(np.dot(project_neck_on_trunk_plane, project_neck_on_trunk_plane)) * math.sqrt( 50 | np.dot(spine_vector, spine_vector))))) 51 | 52 | self.file.write("Neck side bending \n") 53 | self.file.write(str(neck_side_bending)) 54 | self.file.write("\n") 55 | 56 | def neck_torsion_calculator(self): 57 | # TODO: I should calculate the rotation 58 | m_body_number = bodyNum.BodyPartNumber() 59 | neck_joint_numbers = m_body_number.neck() 60 | q1 = self.orientation[neck_joint_numbers[1]] 61 | q2 = self.orientation[neck_joint_numbers[0]] 62 | # finding the rotor that express rotation between two orientational frame(between outer and inner joint) 63 | rotor = util.find_rotation_quaternion(q1, q2) 64 | neck_twist = math.acos(rotor[0]) * 2 * (180 / np.pi) 65 | self.file.write("Neck twist \n") 66 | self.file.write(str(0)) 67 | self.file.write(str(neck_twist)) 68 | self.file.write("\n") 69 | return neck_twist 70 | 71 | -------------------------------------------------------------------------------- /fabrik_full_body/angle_calculator/leg.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import fabrik_full_body.angle_calculator.body_part_numbering as bodyNum 4 | 5 | 6 | class Leg: 7 | def __init__(self, joints,file): 8 | self.joints = joints 9 | self.file = file 10 | 11 | def leg_degree(self): 12 | m_body_number = bodyNum.BodyPartNumber() 13 | right_leg_joint_numbers = m_body_number.right_leg() 14 | left_leg_joint_numbers = m_body_number.left_leg() 15 | right_knee_hip_vector = self.joints[right_leg_joint_numbers[0]] - self.joints[right_leg_joint_numbers[1]] 16 | right_ankle_knee_vector = self.joints[right_leg_joint_numbers[1]] - self.joints[right_leg_joint_numbers[2]] 17 | 18 | left_knee_hip_vector = self.joints[left_leg_joint_numbers[0]] - self.joints[left_leg_joint_numbers[1]] 19 | left_ankle_knee_vector = self.joints[left_leg_joint_numbers[1]] - self.joints[left_leg_joint_numbers[2]] 20 | 21 | # knee degree 22 | right_knee_degree = math.degrees(math.acos(np.dot(right_knee_hip_vector, right_ankle_knee_vector) / ( 23 | (math.sqrt(np.dot(right_knee_hip_vector, right_knee_hip_vector))) * ( 24 | math.sqrt(np.dot(right_ankle_knee_vector, right_ankle_knee_vector)))))) 25 | 26 | left_knee_degree = math.degrees(math.acos(np.dot(left_knee_hip_vector, left_ankle_knee_vector) / ( 27 | (math.sqrt(np.dot(left_knee_hip_vector, left_knee_hip_vector))) * ( 28 | math.sqrt(np.dot(left_ankle_knee_vector, left_ankle_knee_vector)))))) 29 | 30 | self.file.write("Upper right leg flexion \n") 31 | self.file.write(str(right_knee_degree)) 32 | self.file.write("\n") 33 | 34 | self.file.write("Upper left leg flexion \n") 35 | self.file.write(str(left_knee_degree)) 36 | self.file.write("\n") 37 | 38 | # we consider both feet are fixed on ground 39 | self.file.write("Upper right leg abduction \n") 40 | self.file.write(str(0)) 41 | self.file.write("\n") 42 | 43 | self.file.write("Upper left leg abduction \n") 44 | self.file.write(str(0)) 45 | self.file.write("\n") 46 | 47 | self.file.write("Lower right leg flexion \n") 48 | self.file.write(str(0)) 49 | self.file.write("\n") 50 | 51 | self.file.write("Lower left leg flexion \n") 52 | self.file.write(str(0)) 53 | self.file.write("\n") 54 | 55 | self.file.write("Right foot flexion \n") 56 | self.file.write(str(0)) 57 | self.file.write("\n") 58 | 59 | self.file.write("Left foot flexion \n") 60 | self.file.write(str(0)) 61 | self.file.write("\n") 62 | 63 | self.file.write("Right foot bending \n") 64 | self.file.write(str(0)) 65 | self.file.write("\n") 66 | 67 | self.file.write("Left foot bending \n") 68 | self.file.write(str(0)) 69 | self.file.write("\n") 70 | 71 | # rotation should be written 72 | self.file.write("Right foot twist \n") 73 | self.file.write(str(0)) 74 | self.file.write("\n") 75 | 76 | self.file.write("Left foot twist \n") 77 | self.file.write(str(0)) 78 | self.file.write("\n") 79 | 80 | self.file.write("Upper right leg rotation \n") 81 | self.file.write(str(0)) 82 | self.file.write("\n") 83 | 84 | self.file.write("Upper left leg rotation \n") 85 | self.file.write(str(0)) 86 | self.file.write("\n") -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 | # FULL HUMAN BODY INVERSE KINEMATIC SIMULATION BY FABRIK 3 | 4 | # Description: 5 | 6 | This library is an implementation of the FABRIK method(Forward And Backward Reaching Inverse kinematic) 7 | for whole human body in the Python programming language. and is released under the MIT software license 8 | and is under developement. It can simulate the human behavior in reaching a target with consideration of 9 | the human joints constraints. 10 | NOTE: This library is under developement so there is some update in future. 11 | 12 | # Install 13 | You can install the `fabrik` library using the following command: 14 | 15 | pip3 install git+https://github.com/Atiehmerikh/FABRIK_python.git 16 | ## Usage: 17 | 18 | To use the code you should specify some values in text files (see `test` folder in the root): 19 | First you should consider the numbering in `body_number.jpg` file and rest of input files 20 | are based on these numbering 21 | 22 | 1. `joits_position.txt` : This is the initial position of the joints 23 | 2. `joints_constraint.txt`: This is the constraints for each human body joints. According to the joint reference plane and coordinate, you should enter four number for each joint (Adduction, Abduction, Flexion, Extension).Notice that order of these four number is according to joints orientation in its reference plane(in degree). 24 | 3. `orientation.txt`: This is the initial orientation of each joint in quaternion 25 | 4. `bone_twist_constraints.txt`: The twist limitations for each bone. The limitation for each bone should be based on the outer bone number. For anyone that you don't have data let it be on its default. 26 | 5. `constraint_type.txt`: File for getting the joints constraint ("BALL": for ball and socket) and ("hinge" : for hinge type) 27 | 6. `target.txt`: First line of this file specifies the target position and the second line represents the orientation of the target 28 | 29 | After creating these files inside the `input` folder (see `test` folder) and creating `output` folder (it should be empty first), you can test the FABRIK method like the following: 30 | 31 | ```python 32 | from fabrik_full_body import fabrik, input_reader 33 | 34 | reader = input_reader.InputReader() 35 | manipulator = fabrik.FABRIK(reader.joints(), 36 | reader.initial_joints_position(), 37 | reader.orientation(), 38 | reader.target_position(), 39 | reader.target_orientation(), 40 | reader.joints_constraints(), 41 | reader.constraint_type(), 42 | reader.bone_orientation_limit()) 43 | manipulator.solve() 44 | ``` 45 | 46 | ## OutPut: 47 | 48 | Output is a text file (`output/angles.txt`) which computed joint angles and also the final position in stick diagram. 49 | 50 | ## References: 51 | 52 | The FABRIK algorithm is explained in the following research paper: 53 | 54 | Aristidou, A., & Lasenby, J. (2011). FABRIK: a fast, iterative solver for the inverse kinematics problem. Graphical Models, 73(5), 243-260. 55 | 56 | ## Publication: 57 | By using this library this paper is published. Please cite it if you used this library: 58 | 59 | Atieh Merikh-Nejadasl, Ilias El Makrini, Greet Van De Perre, Tom Verstraten, Bram Vanderborght, 60 | A generic algorithm for computing optimal ergonomic postures during working in an industrial environment, 61 | International Journal of Industrial Ergonomics, 62 | Volume 84, 63 | 2021, 64 | 103145, 65 | ISSN 0169-8141, 66 | https://doi.org/10.1016/j.ergon.2021.103145. 67 | (https://www.sciencedirect.com/science/article/pii/S0169814121000639) 68 | Abstract: The present study tries to decrease the risk of work-related musculoskeletal disorders for industry workers by proposing a generic algorithm that recommends an optimal ergonomic posture for accomplishing tasks in an industrial environment. In the case of a dangerous ergonomic pose, the optimization algorithm starts by heuristically changing it to a more ergonomic one. Each recommended posture's feasibility is tested with an inverse kinematic method that can predict the worker's behavior for accomplishing a task. This iterative optimization procedure continues until the optimal ergonomic pose for the worker is achieved. The algorithm's validity is tested in thirteen cases, people with different gender (50 percent male, 50 percent female) aged between 20 and 35, and different height and body morphologies. According to studies, there is a connection between musculoskeletal disorders and the wrong posture for accomplishing tasks in industries. We suggest an optimization algorithm that can indicate the worker the optimal ergonomic pose by considering task constraints in real-time. 69 | Keywords: Ergonomic optimization; FABRIK; REBA; Inverse kinematic; Python 70 | 71 | ## Future Works 72 | 1. solve the algorihm for joints constraint which makes parabolic section in joints reference plane 73 | 2. target locating on the line of kinematic 74 | 3. Handling If the target is unreachable i.e. if the distance between the root and the target is less than the length of the kinematic chain. 75 | 4. multiple end effector 76 | 5. the human feet be able to move 77 | 78 | -------------------------------------------------------------------------------- /fabrik_full_body/draw.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | from mpl_toolkits.mplot3d import axes3d, Axes3D 3 | import numpy as np 4 | import fabrik_full_body.util as util 5 | 6 | 7 | class Draw: 8 | def __init__(self, joints, target, first_pos, rightArmIndex, leftArmIndex, upperChain, lowerChain, rightLeg, 9 | leftLeg, neck, head): 10 | self.joints = joints 11 | self.first_pose = first_pos 12 | self.target = target 13 | self.right_arm_index = rightArmIndex 14 | self.left_arm_index = leftArmIndex 15 | self.upper_chain_index = upperChain 16 | self.lower_chain_index = lowerChain 17 | self.right_leg_index = rightLeg 18 | self.left_leg_index = leftLeg 19 | self.neck_index = neck 20 | self.head_index = head 21 | 22 | def set_axes_radius(self, ax, origin, radius): 23 | ax.set_xlim3d([origin[0] - radius, origin[0] + radius]) 24 | ax.set_ylim3d([origin[1] - radius, origin[1] + radius]) 25 | ax.set_zlim3d([origin[2] - radius, origin[2] + radius]) 26 | 27 | def set_axes_equal(self, ax): 28 | limits = np.array([ 29 | ax.get_xlim3d(), 30 | ax.get_ylim3d(), 31 | ax.get_zlim3d(), 32 | ]) 33 | 34 | origin = np.mean(limits, axis=1) 35 | radius = 0.5 * np.max(np.abs(limits[:, 1] - limits[:, 0])) 36 | self.set_axes_radius(ax, origin, radius) 37 | 38 | def fill_array(self, body_part_index, joints): 39 | w, h = 3, len(body_part_index) 40 | coordinate = [[0 for x in range(w)] for y in range(h)] 41 | # coordinate =[][len(body_part_index)] 42 | x = [] 43 | y = [] 44 | z = [] 45 | for i in range(len(body_part_index)): 46 | x.append(joints[body_part_index[i]][0]) 47 | y.append(joints[body_part_index[i]][1]) 48 | z.append(joints[body_part_index[i]][2]) 49 | coordinate[0][:] = x 50 | coordinate[1][:] = y 51 | coordinate[2][:] = z 52 | return coordinate 53 | 54 | def draw_final(self): 55 | # The Second Pose 56 | coordinate = self.fill_array(self.right_arm_index, self.joints) 57 | x_prime_right_arm = coordinate[0] 58 | y_prime_right_arm = coordinate[1] 59 | z_prime_right_arm = coordinate[2] 60 | 61 | coordinate = self.fill_array(self.left_arm_index, self.joints) 62 | x_primeLArm = coordinate[0] 63 | y_primeLArm = coordinate[1] 64 | z_primeLArm = coordinate[2] 65 | 66 | coordinate = self.fill_array([0, 1, 5,0], self.joints) 67 | x_primeU = coordinate[0] 68 | y_primeU = coordinate[1] 69 | z_primeU = coordinate[2] 70 | 71 | coordinate = self.fill_array(self.lower_chain_index, self.joints) 72 | x_primeL = coordinate[0] 73 | y_primeL = coordinate[1] 74 | z_primeL = coordinate[2] 75 | 76 | coordinate = self.fill_array(self.right_leg_index, self.joints) 77 | x_primeRLeg = coordinate[0] 78 | y_primeRLeg = coordinate[1] 79 | z_primeRLeg = coordinate[2] 80 | 81 | coordinate = self.fill_array(self.left_leg_index, self.joints) 82 | x_primeLLeg = coordinate[0] 83 | y_primeLLeg = coordinate[1] 84 | z_primeLLeg = coordinate[2] 85 | 86 | # # ............................................................................................... 87 | coordinate = self.fill_array(self.right_arm_index, self.first_pose) 88 | x_RArm = coordinate[0] 89 | y_RArm = coordinate[1] 90 | z_RArm = coordinate[2] 91 | 92 | coordinate = self.fill_array(self.left_arm_index, self.first_pose) 93 | x_LArm = coordinate[0] 94 | y_LArm = coordinate[1] 95 | z_LArm = coordinate[2] 96 | 97 | coordinate = self.fill_array([0, 1, 5,0], self.first_pose) 98 | x_U = coordinate[0] 99 | y_U = coordinate[1] 100 | z_U = coordinate[2] 101 | 102 | coordinate = self.fill_array(self.lower_chain_index, self.first_pose) 103 | x_L = coordinate[0] 104 | y_L = coordinate[1] 105 | z_L = coordinate[2] 106 | 107 | coordinate = self.fill_array(self.right_leg_index, self.first_pose) 108 | x_RLeg = coordinate[0] 109 | y_RLeg = coordinate[1] 110 | z_RLeg = coordinate[2] 111 | 112 | coordinate = self.fill_array(self.left_leg_index, self.first_pose) 113 | x_LLeg = coordinate[0] 114 | y_LLeg = coordinate[1] 115 | z_LLeg = coordinate[2] 116 | 117 | fig = plt.figure() 118 | ax = fig.gca(projection='3d') 119 | 120 | ax.plot3D(x_RArm, y_RArm, z_RArm, color='red') 121 | ax.plot3D(x_LArm, y_LArm, z_LArm, color='red') 122 | ax.plot3D(x_U, y_U, z_U, color='red') 123 | ax.plot3D(x_L, y_L, z_L, color='red') 124 | ax.plot3D(x_RLeg, y_RLeg, z_RLeg, color='red') 125 | ax.plot3D(x_LLeg, y_LLeg, z_LLeg, color='red') 126 | # # ax.plot3D(x_Neck, y_Neck, z_Neck, color='red') 127 | # # ax.plot3D(x_head, y_head, z_head, color='red') 128 | 129 | ax.plot3D(x_prime_right_arm, y_prime_right_arm, 130 | z_prime_right_arm, color='green') 131 | ax.plot3D(x_primeLArm, y_primeLArm, z_primeLArm, color='green') 132 | ax.plot3D(x_primeU, y_primeU, z_primeU, color='green') 133 | ax.plot3D(x_primeL, y_primeL, z_primeL, color='green') 134 | ax.plot3D(x_primeRLeg, y_primeRLeg, z_primeRLeg, color='green') 135 | ax.plot3D(x_primeLLeg, y_primeLLeg, z_primeLLeg, color='green') 136 | # ax.plot3D(x_primeNeck, y_primeNeck, z_primeNeck, color='green') 137 | # ax.plot3D(x_primeHead, y_primeHead, z_primeHead, color='green') 138 | 139 | # FrightUpperArmLength = distance_calculation(joints[0], joints[9]) 140 | # LrightUpperArmLength = distance_calculation(first_pos[0], first_pos[9]) 141 | # 142 | # # ax.scatter3D(joints[head[1]][0], joints[head[1]][1], joints[head[1]][2],edgecolors='green') 143 | ax.scatter3D(self.target[0], self.target[1], self.target[2]) 144 | # 145 | self.set_axes_equal(ax) 146 | plt.show() 147 | -------------------------------------------------------------------------------- /fabrik_full_body/angle_calculator/wrist.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | import fabrik_full_body.util as util 4 | import fabrik_full_body.angle_calculator.body_part_numbering as bodyNum 5 | 6 | 7 | class Wrist: 8 | def __init__(self, joints,orientation,file): 9 | self.joints = joints 10 | self.file = file 11 | self.orientation= orientation 12 | 13 | def wrist_flex(self): 14 | m_body = bodyNum.BodyPartNumber() 15 | right_arm_joint_number = m_body.right_arm() 16 | left_arm_joint_number = m_body.left_arm() 17 | 18 | right_shoulder_elbow_vector = self.joints[right_arm_joint_number[1]] - self.joints[right_arm_joint_number[0]] 19 | left_shoulder_elbow_vector = self.joints[left_arm_joint_number[1]] - self.joints[left_arm_joint_number[0]] 20 | right_elbow_wrist_vector = self.joints[right_arm_joint_number[2]] - self.joints[right_arm_joint_number[1]] 21 | left_elbow_wrist_vector = self.joints[left_arm_joint_number[2]] - self.joints[left_arm_joint_number[1]] 22 | right_wrist_finger_vector = self.joints[right_arm_joint_number[3]] - self.joints[right_arm_joint_number[2]] 23 | left_wrist_finger_vector = self.joints[left_arm_joint_number[3]] - self.joints[left_arm_joint_number[2]] 24 | 25 | right_plane_normal_vec = np.cross(right_shoulder_elbow_vector, right_elbow_wrist_vector) 26 | left_plane_normal_vec = np.cross(left_shoulder_elbow_vector, left_elbow_wrist_vector) 27 | 28 | right_wrist_flex = math.degrees(math.acos(np.dot(right_elbow_wrist_vector, right_wrist_finger_vector) / ( 29 | math.sqrt(np.dot(right_elbow_wrist_vector, right_elbow_wrist_vector)) * math.sqrt( 30 | np.dot(right_wrist_finger_vector, right_wrist_finger_vector))))) 31 | left_wrist_flex = math.degrees(math.acos(np.dot(left_elbow_wrist_vector, left_wrist_finger_vector) / ( 32 | math.sqrt(np.dot(left_elbow_wrist_vector, left_elbow_wrist_vector)) * math.sqrt( 33 | np.dot(left_wrist_finger_vector, left_wrist_finger_vector))))) 34 | 35 | if right_plane_normal_vec[0] != 0 or right_plane_normal_vec[1] != 0 or right_plane_normal_vec[2] != 0: 36 | if np.dot(np.cross(right_wrist_finger_vector, right_elbow_wrist_vector), right_plane_normal_vec) > 0: 37 | # means extend of wrist 38 | right_wrist_flex *= -1 39 | 40 | if left_plane_normal_vec[0] != 0 or left_plane_normal_vec[1] != 0 or left_plane_normal_vec[2] == 0: 41 | if np.dot(np.cross(left_wrist_finger_vector, left_elbow_wrist_vector), left_plane_normal_vec) > 0: 42 | # means extend of wrist 43 | left_wrist_flex *= -1 44 | 45 | self.file.write("Right hand flexion \n") 46 | self.file.write(str(right_wrist_flex)) 47 | self.file.write("\n") 48 | 49 | self.file.write("Left hand flexion \n") 50 | self.file.write(str(left_wrist_flex)) 51 | self.file.write("\n") 52 | 53 | def wrist_side(self): 54 | m_body = bodyNum.BodyPartNumber() 55 | right_arm_joint_number = m_body.right_arm() 56 | left_arm_joint_number = m_body.left_arm() 57 | 58 | right_shoulder_elbow_vector = self.joints[right_arm_joint_number[1]] - self.joints[right_arm_joint_number[0]] 59 | left_shoulder_elbow_vector = self.joints[left_arm_joint_number[1]] - self.joints[left_arm_joint_number[0]] 60 | right_elbow_wrist_vector = self.joints[right_arm_joint_number[2]] - self.joints[right_arm_joint_number[1]] 61 | left_elbow_wrist_vector = self.joints[left_arm_joint_number[2]] - self.joints[left_arm_joint_number[1]] 62 | right_wrist_finger_vector = self.joints[right_arm_joint_number[3]] - self.joints[right_arm_joint_number[2]] 63 | left_wrist_finger_vector = self.joints[left_arm_joint_number[3]] - self.joints[left_arm_joint_number[2]] 64 | 65 | right_plane_normal_vec = np.cross(right_shoulder_elbow_vector, right_elbow_wrist_vector) 66 | left_plane_normal_vec = np.cross(left_shoulder_elbow_vector, left_elbow_wrist_vector) 67 | 68 | if right_plane_normal_vec[0] != 0 or right_plane_normal_vec[1] != 0 or right_plane_normal_vec[2] != 0: 69 | right_side_bent_degree = 90 - math.degrees( 70 | math.acos(np.dot(right_plane_normal_vec, right_wrist_finger_vector) / ( 71 | math.sqrt(np.dot(right_wrist_finger_vector, right_wrist_finger_vector)) * math.sqrt( 72 | np.dot(right_plane_normal_vec, right_plane_normal_vec))))) 73 | else: 74 | right_side_bent_degree = 0 75 | if left_plane_normal_vec[0] != 0 or left_plane_normal_vec[1] != 0 or left_plane_normal_vec[2] == 0: 76 | left_side_bent_degree = 90 - math.degrees( 77 | math.acos(np.dot(left_plane_normal_vec, left_wrist_finger_vector) / ( 78 | math.sqrt(np.dot(left_wrist_finger_vector, left_wrist_finger_vector)) * math.sqrt( 79 | np.dot(left_plane_normal_vec, left_plane_normal_vec))))) 80 | else: 81 | left_side_bent_degree = 0 82 | 83 | self.file.write("Right hand bending \n") 84 | self.file.write(str(right_side_bent_degree)) 85 | self.file.write("\n") 86 | 87 | self.file.write("Left hand bending \n") 88 | self.file.write(str(left_side_bent_degree)) 89 | self.file.write("\n") 90 | 91 | def wrist_torsion(self): 92 | m_body_number = bodyNum.BodyPartNumber() 93 | right_wrist_joint_numbers = m_body_number.right_arm() 94 | q1 = self.orientation[right_wrist_joint_numbers[3]] 95 | q2 = self.orientation[right_wrist_joint_numbers[2]] 96 | # finding the rotor that express rotation between two orientational frame(between outer and inner joint) 97 | rotor = util.find_rotation_quaternion(q1, q2) 98 | right_wrist_twist = math.acos(rotor[0]) * 2 * (180 / np.pi) 99 | 100 | m_body_number = bodyNum.BodyPartNumber() 101 | left_wrist_joint_numbers = m_body_number.left_arm() 102 | q1 = self.orientation[left_wrist_joint_numbers[3]] 103 | q2 = self.orientation[left_wrist_joint_numbers[2]] 104 | # finding the rotor that express rotation between two orientational frame(between outer and inner joint) 105 | rotor = util.find_rotation_quaternion(q1, q2) 106 | left_wrist_twist = math.acos(rotor[0]) * 2 * (180 / np.pi) 107 | 108 | self.file.write("Right hand twist \n") 109 | self.file.write(str(right_wrist_twist)) 110 | self.file.write("\n") 111 | 112 | self.file.write("Left hand twist \n") 113 | self.file.write(str(left_wrist_twist)) 114 | self.file.write("\n") 115 | -------------------------------------------------------------------------------- /fabrik_full_body/angle_calculator/upper_arm.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import fabrik_full_body.angle_calculator.body_part_numbering as bodyNum 4 | 5 | 6 | class UpperArm: 7 | def __init__(self, joints, file): 8 | self.joints = joints 9 | self.file = file 10 | 11 | def trunk_plane(self): 12 | m_body_number = bodyNum.BodyPartNumber() 13 | trunk_joint_numbers = m_body_number.trunk_upper_body() 14 | 15 | # finding a plane of upper body 16 | u = self.joints[trunk_joint_numbers[1]] - self.joints[trunk_joint_numbers[0]] 17 | v = self.joints[trunk_joint_numbers[3]] - self.joints[trunk_joint_numbers[0]] 18 | 19 | normal_plane = np.cross(u, v) 20 | return normal_plane 21 | 22 | def upper_arm_flex(self): 23 | m_body_number = bodyNum.BodyPartNumber() 24 | right_upper_arm_joint_numbers = m_body_number.right_arm() 25 | left_upper_arm_joint_numbers = m_body_number.left_arm() 26 | 27 | right_upper_arm_vector = self.joints[right_upper_arm_joint_numbers[2]] - self.joints[ 28 | right_upper_arm_joint_numbers[1]] 29 | 30 | left_upper_arm_vector = self.joints[left_upper_arm_joint_numbers[2]] - self.joints[ 31 | left_upper_arm_joint_numbers[1]] 32 | 33 | normal_trunk_plane = self.trunk_plane() 34 | 35 | flex_right_upper_arm = 90 - math.degrees(math.acos(np.dot(right_upper_arm_vector, normal_trunk_plane) / 36 | (math.sqrt( 37 | np.dot(normal_trunk_plane, 38 | normal_trunk_plane)) * math.sqrt( 39 | np.dot(right_upper_arm_vector, 40 | right_upper_arm_vector))))) 41 | 42 | flex_left_upper_arm = 90 - math.degrees(math.acos(np.dot(left_upper_arm_vector, normal_trunk_plane) / 43 | (math.sqrt( 44 | np.dot(normal_trunk_plane, 45 | normal_trunk_plane)) * math.sqrt( 46 | np.dot(left_upper_arm_vector, 47 | left_upper_arm_vector))))) 48 | 49 | self.file.write("Upper right arm flexion \n") 50 | self.file.write(str(flex_right_upper_arm)) 51 | self.file.write("\n") 52 | 53 | self.file.write("Upper left arm flexion \n") 54 | self.file.write(str(flex_left_upper_arm)) 55 | self.file.write("\n") 56 | 57 | def upper_arm_side_bending(self): 58 | m_body_number = bodyNum.BodyPartNumber() 59 | right_upper_arm_joint_numbers = m_body_number.right_arm() 60 | left_upper_arm_joint_numbers = m_body_number.left_arm() 61 | 62 | trunk_joint_numbers = m_body_number.trunk_upper_body() 63 | right_upper_arm_vector = self.joints[right_upper_arm_joint_numbers[2]] - self.joints[ 64 | right_upper_arm_joint_numbers[1]] 65 | left_upper_arm_vector = self.joints[left_upper_arm_joint_numbers[2]] - self.joints[ 66 | left_upper_arm_joint_numbers[1]] 67 | 68 | normal_trunk_plane = self.trunk_plane() 69 | 70 | proj_right_upperarm_on_plane = right_upper_arm_vector - np.dot(right_upper_arm_vector, 71 | normal_trunk_plane) * normal_trunk_plane 72 | 73 | proj_left_upperarm_on_plane = left_upper_arm_vector - np.dot(left_upper_arm_vector, 74 | normal_trunk_plane) * normal_trunk_plane 75 | 76 | spine_vector = self.joints[trunk_joint_numbers[0]] - self.joints[trunk_joint_numbers[2]] 77 | 78 | right_side_degree = np.dot(spine_vector, proj_right_upperarm_on_plane) / ( 79 | math.sqrt(np.dot(spine_vector, spine_vector)) * math.sqrt( 80 | np.dot(proj_right_upperarm_on_plane, proj_right_upperarm_on_plane))) 81 | 82 | left_side_degree = np.dot(spine_vector, proj_left_upperarm_on_plane) / ( 83 | math.sqrt(np.dot(spine_vector, spine_vector)) * math.sqrt( 84 | np.dot(proj_left_upperarm_on_plane, proj_left_upperarm_on_plane))) 85 | 86 | if np.dot(np.cross(spine_vector, right_upper_arm_vector), normal_trunk_plane) < 0: 87 | # if the arm go to the body: adduction 88 | right_side_degree *= -1 89 | 90 | if np.dot(np.cross(spine_vector, left_upper_arm_vector), normal_trunk_plane) > 0: 91 | left_side_degree *= -1 92 | 93 | self.file.write("Upper right arm abduction \n") 94 | self.file.write(str(right_side_degree)) 95 | self.file.write("\n") 96 | 97 | self.file.write("Upper left arm abduction \n") 98 | self.file.write(str(left_side_degree)) 99 | self.file.write("\n") 100 | 101 | def shoulder_rise(self): 102 | m_body_number = bodyNum.BodyPartNumber() 103 | trunk_joint_numbers = m_body_number.trunk_upper_body() 104 | right_shoulder_joint_numbers = m_body_number.right_shoulder() 105 | left_shoulder_joint_numbers = m_body_number.left_shoulder() 106 | spine_vector = self.joints[trunk_joint_numbers[0]] - self.joints[trunk_joint_numbers[2]] 107 | right_shoulder_vector = self.joints[right_shoulder_joint_numbers[1]] - self.joints[ 108 | right_shoulder_joint_numbers[0]] 109 | left_shoulder_vector = self.joints[left_shoulder_joint_numbers[1]] - self.joints[left_shoulder_joint_numbers[0]] 110 | 111 | right_shoulder_rise_degree = math.degrees(math.acos(np.dot(spine_vector, right_shoulder_vector) / ( 112 | math.sqrt(np.dot(right_shoulder_vector, right_shoulder_vector)) * math.sqrt( 113 | np.dot(spine_vector, spine_vector))))) 114 | left_shoulder_rise_degree = math.degrees(math.acos(np.dot(spine_vector, left_shoulder_vector) / ( 115 | math.sqrt(np.dot(left_shoulder_vector, left_shoulder_vector)) * math.sqrt( 116 | np.dot(spine_vector, spine_vector))))) 117 | 118 | self.file.write("Upper right arm raise \n") 119 | self.file.write(str(right_shoulder_rise_degree)) 120 | self.file.write("\n") 121 | 122 | self.file.write("Upper left arm raise \n") 123 | self.file.write(str(left_shoulder_rise_degree)) 124 | self.file.write("\n") 125 | 126 | def upper_arm_rotation(self): 127 | # TODO: twist should be coded 128 | self.file.write("Upper right arm rotation \n") 129 | self.file.write(str(0)) 130 | self.file.write("\n") 131 | 132 | self.file.write("Upper left arm rotation \n") 133 | self.file.write(str(0)) 134 | self.file.write("\n") 135 | -------------------------------------------------------------------------------- /fabrik_full_body/constraints.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | import fabrik_full_body.util as util 4 | 5 | 6 | class Constraints: 7 | 8 | def __init__(self, joints, body_index, i, theta, length, constraint_type, orientation): 9 | self.joints = joints 10 | self.body_index = body_index 11 | self.i = i 12 | self.pi = 22 / 7 13 | self.theta = (self.pi / 180) * theta 14 | self.length = length 15 | self.constraint_type = constraint_type 16 | # the orientation of joint 17 | theta = math.acos(orientation[0]) * 2 18 | self.si = theta 19 | 20 | def rotational_constraint(self): 21 | p_i = [self.joints[self.body_index[self.i]][0], self.joints[self.body_index[self.i]][1], 22 | self.joints[self.body_index[self.i]][2]] 23 | p_before = [self.joints[self.body_index[self.i - 1]][0], self.joints[self.body_index[self.i - 1]][1], 24 | self.joints[self.body_index[self.i - 1]][2]] 25 | p_next = [self.joints[self.body_index[self.i + 1]][0], self.joints[self.body_index[self.i + 1]][1], 26 | self.joints[self.body_index[self.i + 1]][2]] 27 | 28 | v_i_next = [p_next[0] - p_i[0], p_next[1] - p_i[1], p_next[2] - p_i[2]] 29 | v_before_i = [p_i[0] - p_before[0], p_i[1] - p_before[1], p_i[2] - p_before[2]] 30 | 31 | dot_prod = np.inner(v_i_next, v_before_i) 32 | s = dot_prod 33 | 34 | # a unit vector of a line passing from p(i+1) and P(i) 35 | l_next_i = util.distance(p_next, p_i) 36 | unit_vec_next_i = [(p_i[0] - p_next[0]) / l_next_i, 37 | (p_i[1] - p_next[1]) / l_next_i, 38 | (p_i[2] - p_next[2]) / l_next_i] 39 | 40 | # the center of cone 41 | o = [p_i[0] + unit_vec_next_i[0] * s, p_i[1] + unit_vec_next_i[1] * s, 42 | p_i[2] + unit_vec_next_i[2] * s] 43 | 44 | if (self.constraint_type == "hinge"): 45 | # normal plane vector of p(next), p(i), p(before) 46 | normal_plane = np.cross(v_i_next, v_before_i) 47 | l = math.sqrt(np.inner(normal_plane, normal_plane)) 48 | uv_normal_plane = [normal_plane[0] / l, normal_plane[1] / l, normal_plane[2] / l] 49 | # a vector from p_i to o 50 | l_i__o = util.distance(p_i, o) 51 | unit_vec_i_o = [(o[0] - p_i[0]) / l_i__o, 52 | (o[1] - p_i[1]) / l_i__o, 53 | (o[2] - p_i[2]) / l_i__o] 54 | 55 | axis_normal_plane = [uv_normal_plane[0], uv_normal_plane[1], uv_normal_plane[2]] 56 | unit_vec_i_o_array = [unit_vec_i_o[0], unit_vec_i_o[1], unit_vec_i_o[2]] 57 | # rotating p(i)-o C.C.W to find flexion constraint(left of pi_o) 58 | p_down = self.times(self.rotation_matrix(axis_normal_plane, self.theta[1]), unit_vec_i_o_array) 59 | p_down = [p_down[0], p_down[1], p_down[2]] 60 | # rotating p(i)-o C.W to find extension constraint(right of pi_o ) 61 | p_up = self.times(self.rotation_matrix(axis_normal_plane, -self.theta[3]), unit_vec_i_o_array) 62 | p_up = [p_up[0], p_up[1], p_up[2]] 63 | 64 | l_before_i = util.distance(p_i, p_before) 65 | uv_i_before = [(p_before[0] - p_i[0]) / l_before_i, (p_before[1] - p_i[1]) / l_before_i, 66 | (p_before[2] - p_i[2]) / l_before_i] 67 | 68 | if np.inner(np.cross(uv_i_before, unit_vec_i_o), (uv_normal_plane)) > 0: 69 | # means it is upper side of pi_o 70 | if self.theta[3] != 0: 71 | # angle between vec(i_before) and vec(i_o) 72 | angle = math.acos(np.inner(uv_i_before, unit_vec_i_o)) 73 | if angle <= self.theta[3]: 74 | return [0, 0, 0] 75 | else: 76 | p_nearest_point = p_i + l_before_i * p_up 77 | return p_nearest_point 78 | else: 79 | p_nearest_point = [p_i[0] + unit_vec_i_o[0] * l_before_i, 80 | p_i[1] + unit_vec_i_o[1] * l_before_i, 81 | p_i[2] + unit_vec_i_o[2] * l_before_i] 82 | return p_nearest_point 83 | else: 84 | # means it is down side of pi_o 85 | if self.theta[1] != 0: 86 | # angle between vec(i_before) and vec(i_o) 87 | angle = math.acos(np.inner(uv_i_before, unit_vec_i_o)) 88 | if angle <= self.theta[1]: 89 | return [0, 0, 0] 90 | else: 91 | p_nearest_point = p_i + np.dot(l_before_i, p_down) 92 | return p_nearest_point 93 | else: 94 | p_nearest_point = [p_i[0] + np.dot(unit_vec_i_o[0] , l_before_i), 95 | p_i[1] + np.dot(unit_vec_i_o[1] , l_before_i), 96 | p_i[2] + np.dot(unit_vec_i_o[2] , l_before_i)] 97 | return p_nearest_point 98 | if self.constraint_type == "BALL": 99 | # Semi ellipsoidal parameter qi (1,2,3,4) 100 | q1 = round(s * math.tan(self.theta[0]), 3) 101 | q2 = round(s * math.tan(self.theta[1]), 3) 102 | q3 = round(s * math.tan(self.theta[2]), 3) 103 | q4 = round(s * math.tan(self.theta[3]), 3) 104 | 105 | # change the coordinate to cross section of cone and calculating the (i-1)th position in it 106 | l_o_next = util.distance(o, p_next) 107 | 108 | if 0 <= round(self.si) < np.pi / 2: 109 | sector = 1 110 | elif np.pi / 2 <= round(self.si) < np.pi: 111 | sector = 2 112 | elif np.pi <= round(self.si) < 3 * np.pi / 2: 113 | sector = 3 114 | elif 3 * np.pi / 2 <= round(self.si) < np.pi / 2 * np.pi: 115 | sector = 4 116 | 117 | y_t = round(l_o_next * math.sin(self.si), 2) 118 | x_t = round(l_o_next * math.cos(self.si), 2) 119 | 120 | # checking that the target point is in ellipsoidal shape 121 | inbound = 0 122 | if round(((x_t ** 2) / (q3 ** 2) + (y_t ** 2) / (q2 ** 2))) <= 1 and sector == 1: 123 | inbound = 1 124 | p_prime = [0, 0, 0] 125 | return p_prime 126 | elif round(((x_t ** 2) / (q1 ** 2) + (y_t ** 2) / (q2 ** 2))) <= 1 and sector == 2: 127 | inbound = 1 128 | p_prime = [0, 0, 0] 129 | return p_prime 130 | elif round(((x_t ** 2) / (q1 ** 2) + (y_t ** 2) / (q4 ** 2))) <= 1 and sector == 3: 131 | inbound = 1 132 | p_prime = [0, 0, 0] 133 | return p_prime 134 | elif round(((x_t ** 2) / (q3 ** 2) + (y_t ** 2) / (q4 ** 2))) <= 1 and sector == 4: 135 | inbound = 1 136 | p_prime = [0, 0, 0] 137 | return p_prime 138 | 139 | # if it is out bound of the ellipsoidal shape we should find the nearest point on ellipsoidal shape 140 | if inbound == 0 and sector == 1: 141 | if y_t != 0: 142 | result = self.find_nearest_point(q3, q2, sector, x_t, y_t) 143 | x_nearest_point = result[0] 144 | y_nearest_point = result[1] 145 | else: 146 | x_nearest_point = q3 147 | y_nearest_point = 0 148 | elif inbound == 0 and sector == 2: 149 | if x_t != 0: 150 | result = self.find_nearest_point(q1, q2, sector, x_t, y_t) 151 | x_nearest_point = result[0] 152 | y_nearest_point = result[1] 153 | else: 154 | x_nearest_point = 0 155 | y_nearest_point = q2 156 | elif inbound == 0 and sector == 3: 157 | if y_t != 0: 158 | result = self.find_nearest_point(q1, q4, sector, x_t, y_t) 159 | x_nearest_point = result[0] 160 | y_nearest_point = result[1] 161 | else: 162 | x_nearest_point = -q1 163 | y_nearest_point = 0 164 | elif inbound == 0 and sector == 4: 165 | if x_t != 0: 166 | result = self.find_nearest_point(q3, q4, sector, x_t, y_t) 167 | x_nearest_point = result[0] 168 | y_nearest_point = result[1] 169 | else: 170 | x_nearest_point = 0 171 | y_nearest_point = -q4 172 | 173 | # finding nearest point global coordinate in 3d 174 | l_o_before = math.sqrt(x_t ** 2 + y_t ** 2) 175 | l_o_nearest_point = math.sqrt(x_nearest_point ** 2 + y_nearest_point ** 2) 176 | l_nearest_before = math.sqrt((x_t - x_nearest_point) ** 2 + (y_nearest_point - y_t) ** 2) 177 | 178 | # rotation angle between vector from o to p_before and o to p_nearest point 179 | rot_angle = (math.acos(round((l_nearest_before ** 2 - l_o_before ** 2 - l_o_nearest_point ** 2) / ( 180 | -2 * l_o_before * l_o_nearest_point)))) 181 | 182 | # rotating the vector from o to p_before around vector from p_next to o 183 | 184 | # a vector from o to p_before 185 | unit_vec_o_before = [(p_before[0] - o[0]) / l_o_before, 186 | (p_before[1] - o[1]) / l_o_before, 187 | (p_before[2] - o[2]) / l_o_before] 188 | unit_vec_normal_plane = [(p_i[0] - p_next[0]) / l_next_i, 189 | (p_i[1] - p_next[1]) / l_next_i, 190 | (p_i[2] - p_next[2]) / l_next_i] 191 | 192 | if rot_angle == 0: 193 | p_nearest_point_in_global = [o[0] + np.dot(l_o_nearest_point, unit_vec_o_before[0]), 194 | o[1] + np.dot(l_o_nearest_point , unit_vec_o_before[1]), 195 | o[2] + np.dot(l_o_nearest_point , unit_vec_o_before[2])] 196 | return p_nearest_point_in_global 197 | else: 198 | # to find out nearest point is on left or right side of target point 199 | orient_near_to_target = x_t * y_nearest_point - x_nearest_point * y_t 200 | unit_vec_o_before = [unit_vec_o_before[0], 201 | unit_vec_o_before[1], 202 | unit_vec_o_before[2]] 203 | if orient_near_to_target * unit_vec_next_i[2] > 0: 204 | # to find nearest point should rotate the uv(o-target) in C.C.W 205 | uv_o_nearest_point = self.times(self.rotation_matrix(unit_vec_normal_plane, rot_angle), 206 | unit_vec_o_before) 207 | else: 208 | uv_o_nearest_point = self.times(self.rotation_matrix(unit_vec_normal_plane, -rot_angle), 209 | unit_vec_o_before) 210 | 211 | p_nearest_point_in_global = [o[0] + np.dot(l_o_nearest_point , uv_o_nearest_point[0]), 212 | o[1] + np.dot(l_o_nearest_point , uv_o_nearest_point[1]), 213 | o[2] + np.dot(l_o_nearest_point , uv_o_nearest_point[2])] 214 | 215 | return p_nearest_point_in_global 216 | 217 | def rotation_matrix(self, axis, theta): 218 | """ 219 | Return the rotation matrix associated with counterclockwise rotation about 220 | the given axis by theta radians. 221 | """ 222 | axis = np.asarray(axis) 223 | axis = axis / math.sqrt(np.dot(axis, axis)) 224 | a = math.cos(theta / 2.0) 225 | b, c, d = -axis * math.sin(theta / 2.0) 226 | aa, bb, cc, dd = a * a, b * b, c * c, d * d 227 | bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d 228 | return [[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)], 229 | [2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)], 230 | [2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]] 231 | 232 | def find_initial_point(self, a, b, sector, x_t, y_t): 233 | x_k1 = x_t * a * b / math.sqrt(b ** 2 * x_t ** 2 + a ** 2 * y_t ** 2) 234 | y_k1 = y_t * a * b / math.sqrt(b ** 2 * x_t ** 2 + a ** 2 * y_t ** 2) 235 | if sector == 1 or sector == 2: 236 | sign_y = 1 237 | elif sector == 3 or sector == 4: 238 | sign_y = -1 239 | if sector == 1 or sector == 4: 240 | sign_x = 1 241 | elif sector == 3 or sector == 2: 242 | sign_x = -1 243 | 244 | if abs(x_t) < a: 245 | x_k2 = x_t 246 | y_k2 = sign_y * (b / a) * math.sqrt(a ** 2 - x_t ** 2) 247 | else: 248 | x_k2 = a * sign_x * a 249 | y_k2 = 0 250 | 251 | x0 = 0.5 * (x_k1 + x_k2) 252 | y0 = 0.5 * (y_k1 + y_k2) 253 | 254 | initial = [x0, y0] 255 | return initial 256 | 257 | def q_matrix(self, x, y, a, b, x_t, y_t): 258 | Q = [[b ** 2 * x, a ** 2 * y], [(a ** 2 - b ** 2) * y + b ** 2 * y_t, (a ** 2 - b ** 2) * x - a ** 2 * x_t]] 259 | return Q 260 | 261 | def times(self, matrix, vector): 262 | return [ 263 | matrix[0][0] * vector[0] + matrix[1][0] * vector[1] + matrix[2][0] * vector[2], 264 | matrix[0][1] * vector[0] + matrix[1][1] * vector[1] + matrix[2][1] * vector[2], 265 | matrix[0][2] * vector[0] + matrix[1][2] * vector[1] + matrix[2][2] * vector[2]] 266 | 267 | def find_delta(self, x, y, a, b, x_t, y_t): 268 | f1 = (a ** 2 * y ** 2 + b ** 2 * x ** 2 - a ** 2 * b ** 2) / 2 269 | f2 = b ** 2 * x * (y_t - y) - a ** 2 * y * (x_t - x) 270 | f = np.matrix([[f1], [f2]]) 271 | Q = self.q_matrix(x, y, a, b, x_t, y_t) 272 | delta = -1 * (np.linalg.inv(Q) * f) 273 | 274 | result = [delta[0, 0], delta[1, 0]] 275 | return result 276 | 277 | def find_next_point(self, x, y, a, b, x_t, y_t): 278 | delta = self.find_delta(x, y, a, b, x_t, y_t) 279 | x = x + delta[0] 280 | y = y + delta[1] 281 | result = [x, y] 282 | return result 283 | 284 | def find_nearest_point(self, a, b, sector, x_t, y_t): 285 | initial = self.find_initial_point(a, b, sector, x_t, y_t) 286 | x_initial = initial[0] 287 | y_initial = initial[1] 288 | x = x_initial 289 | y = y_initial 290 | thresh = 0.001 291 | result = self.find_next_point(x, y, a, b, x_t, y_t) 292 | while result[0] > thresh or result[1] > thresh: 293 | x = result[0] 294 | y = result[1] 295 | result = self.find_next_point(x, y, a, b, x_t, y_t) 296 | x_nearest_point = result[0] 297 | y_nearest_point = result[1] 298 | return [x_nearest_point, y_nearest_point] 299 | -------------------------------------------------------------------------------- /fabrik_full_body/fabrik.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | from fabrik_full_body.constraints import Constraints 4 | from fabrik_full_body.draw import Draw 5 | from fabrik_full_body.angle_calculator.angle_calculator import AngleCalculator 6 | import fabrik_full_body.util as util 7 | from fabrik_full_body.input_reader import * 8 | from fabrik_full_body.output_writer import * 9 | # This is solver of Inverse kinematic of a whole chain of human body with foot on the ground 10 | # For more info about the procedure refer to https://www.sciencedirect.com/science/article/pii/S1524070311000178 11 | # and for the constraints refer to https://www.researchgate.net/publication/271771862_Extending_FABRIK_with_model_constraints 12 | 13 | 14 | class FABRIK: 15 | 16 | def __init__(self, joints, initial_joints_position, orientation, target, target_orientation, theta, constraint_type, 17 | bone_orientation_limit): 18 | self.n = len(joints) 19 | self.joints = joints 20 | self.firstPos = initial_joints_position 21 | self.orientation = orientation 22 | self.target = target 23 | self.targetOrientation = target_orientation 24 | self.constraint_type = constraint_type 25 | self.bone_orientation_limit = bone_orientation_limit 26 | self.Theta = theta 27 | 28 | self.solve_distance_threshold = 0.01 29 | 30 | self.rightArmIndex = [17, 1, 2, 3, 4] 31 | self.right_end_effector_index = 4 32 | self.leftArmIndex = [17, 5, 6, 7, 8] 33 | self.left_end_effector_index = 8 34 | self.upperChain = [0, 1, 5, 17, 0] 35 | self.lowerChain = [0, 9, 10, 0] 36 | self.rightLeg = [13, 12, 11, 9] 37 | self.leftLeg = [16, 15, 14, 10] 38 | self.neck = [0, 17] 39 | self.head = [17, 18] 40 | 41 | self.inverseLeftArmIndex = [8, 7, 6, 5] 42 | self.inverseRightArmIndex = [4, 3, 2, 1] 43 | 44 | # Finding the laterality of the human whole chain 45 | def laterality(self): 46 | """ 47 | Determines right-handed or left-handed person 48 | """ 49 | right_upper_body = util.distance(self.firstPos[self.right_end_effector_index], self.target) 50 | left_upper_body = util.distance(self.firstPos[self.left_end_effector_index], self.target) 51 | 52 | # ON THE RIGHT SIDE OR LEFT relative to how close to end effectors 53 | if right_upper_body <= left_upper_body: 54 | return "right" 55 | if right_upper_body > left_upper_body: 56 | return "left" 57 | 58 | # Finding the next point position in forward/backward iteration of FABRIK 59 | def solve_for_distance(self, i, j, body_part): 60 | r = util.distance(self.joints[body_part[i]], self.joints[body_part[j]]) 61 | landa = util.distance(self.firstPos[body_part[i]], self.firstPos[body_part[j]]) / r 62 | pos = (1 - landa) * self.joints[body_part[i]] + landa * self.joints[body_part[j]] 63 | self.joints[body_part[j]] = pos 64 | 65 | def solve_for_orientation(self, outer_joint, inner_joint, body_part): 66 | q1 = self.orientation[body_part[outer_joint]] 67 | q2 = self.orientation[body_part[inner_joint]] 68 | # finding the rotor that express rotation between two orientational frame(between outer and inner joint) 69 | rotor = self.find_rotation_quaternion(q1, q2) 70 | needed_rotation = math.acos(rotor[0]) * 2 * (180 / np.pi) 71 | 72 | if needed_rotation <= self.bone_orientation_limit[body_part[outer_joint]]: 73 | # if the rotation is inside the limited 74 | self.orientation[body_part[inner_joint]] = self.multiply_two_quaternion(rotor, self.orientation[ 75 | body_part[outer_joint]]) 76 | else: 77 | # the maximum allowed rotation angle 78 | theta = (self.bone_orientation_limit[body_part[outer_joint]]) * (np.pi / 180) 79 | # the rotation axis 80 | v1 = np.dot(rotor[1:], (1 / math.sqrt(1 - rotor[0] ** 2))) 81 | w = math.cos(theta / 2) 82 | x = v1[0] * math.sin(theta / 2) 83 | y = v1[1] * math.sin(theta / 2) 84 | z = v1[2] * math.sin(theta / 2) 85 | self.orientation[body_part[inner_joint]] = [w, x, y, z] 86 | 87 | # Finding the rotation rotor between outer joint and inner joint of each FABRIK iteration 88 | def find_rotation_quaternion(self, outer_quaternion, inner_quaternion): 89 | conjucate = [outer_quaternion[0], -outer_quaternion[1], -outer_quaternion[2], -outer_quaternion[3]] 90 | length = math.sqrt(outer_quaternion[0] ** 2 + outer_quaternion[1] ** 2 + 91 | outer_quaternion[2] ** 2 + outer_quaternion[3] ** 2) 92 | inverse = np.dot(conjucate, (1 / length)) 93 | rotation = self.multiply_two_quaternion(inner_quaternion, inverse) 94 | return rotation 95 | 96 | # multiplication of two quaternion(representing the orientation of joints) 97 | def multiply_two_quaternion(self, q1, q2): 98 | a = q1[0] 99 | b = q1[1] 100 | c = q1[2] 101 | d = q1[3] 102 | e = q2[0] 103 | f = q2[1] 104 | g = q2[2] 105 | h = q2[3] 106 | 107 | m0 = round(a * e - b * f - c * g - d * h, 2) 108 | m1 = round(b * e + a * f + c * h - d * g, 2) 109 | m2 = round(a * g - b * h + c * e + d * f, 2) 110 | m3 = round(a * h + b * g - c * f + d * e, 2) 111 | return [m0, m1, m2, m3] 112 | 113 | def forward_arm(self, arm_index, target, target_orientation): 114 | n = len(arm_index) 115 | # set end effector as target 116 | self.joints[arm_index[n - 1]] = target 117 | self.orientation[arm_index[n - 1]] = target_orientation 118 | for i in range(n - 2, -1, -1): 119 | self.solve_for_distance(i + 1, i, arm_index) 120 | self.solve_for_orientation(i + 1, i, arm_index) 121 | if i < n - 2: 122 | mconstraint = Constraints(self.joints, arm_index, i + 1, self.Theta[arm_index[i + 1]], self.firstPos, 123 | self.constraint_type[arm_index[i + 1]], self.orientation[arm_index[i + 1]]) 124 | constraint_return = mconstraint.rotational_constraint() 125 | if constraint_return[0] != 0: 126 | for j in range(3): 127 | self.joints[arm_index[i]][j] = constraint_return[j] 128 | 129 | def update_upper_chain_f(self, target_position): 130 | if target_position == "right": 131 | self.solve_for_distance(1, 2, self.upperChain) 132 | self.solve_for_distance(2, 0, self.upperChain) 133 | self.solve_for_distance(1, 0, self.upperChain) 134 | self.solve_for_distance(0, 2, self.upperChain) 135 | # for tuning left arm: 136 | self.forward_arm(self.inverseLeftArmIndex, self.joints[self.leftArmIndex[1]], 137 | self.orientation[self.leftArmIndex[1]]) 138 | else: 139 | self.solve_for_distance(2, 0, self.upperChain) 140 | self.solve_for_distance(0, 1, self.upperChain) 141 | self.solve_for_distance(2, 1, self.upperChain) 142 | self.solve_for_distance(1, 0, self.upperChain) 143 | # for tuning right arm: 144 | self.forward_arm(self.inverseRightArmIndex, self.joints[self.rightArmIndex[1]], 145 | self.orientation[self.rightArmIndex[1]]) 146 | 147 | def update_upper_chain_b(self, target_position): 148 | self.solve_for_distance(0, 2, self.upperChain) 149 | self.solve_for_distance(2, 1, self.upperChain) 150 | self.solve_for_distance(0, 1, self.upperChain) 151 | self.solve_for_distance(1, 2, self.upperChain) 152 | if target_position == 'right': 153 | self.forward_arm(self.inverseLeftArmIndex, self.joints[self.leftArmIndex[1]], 154 | self.orientation[self.leftArmIndex[1]]) 155 | else: 156 | self.forward_arm(self.inverseRightArmIndex, self.joints[self.rightArmIndex[1]], 157 | self.orientation[self.rightArmIndex[1]]) 158 | # Neck 159 | self.joints[self.upperChain[3]] = (self.joints[self.upperChain[2]] + self.joints[self.upperChain[1]]) / 2 160 | 161 | def update_lower_chain_f(self): 162 | self.solve_for_distance(0, 1, self.lowerChain) 163 | self.solve_for_distance(1, 2, self.lowerChain) 164 | self.solve_for_distance(0, 2, self.lowerChain) 165 | self.solve_for_distance(2, 1, self.lowerChain) 166 | 167 | def update_lower_chain_b(self): 168 | self.solve_for_distance(1, 0, self.lowerChain) 169 | self.solve_for_distance(2, 0, self.lowerChain) 170 | 171 | def forward_leg(self): 172 | # set end effector of leg as target 173 | n = len(self.rightLeg) 174 | for i in range(n - 2, -1, -1): 175 | self.solve_for_distance(i + 1, i, self.rightLeg) 176 | self.solve_for_distance(i + 1, i, self.leftLeg) 177 | self.solve_for_orientation(i + 1, i, self.rightLeg) 178 | self.solve_for_orientation(i + 1, i, self.leftLeg) 179 | if i < n - 2: 180 | mconstraint = Constraints(self.joints, self.rightLeg, i + 1, self.Theta[self.rightLeg[i + 1]], 181 | self.firstPos, 182 | self.constraint_type[self.rightLeg[i + 1]], 183 | self.orientation[self.rightLeg[i + 1]]) 184 | constraint_return = mconstraint.rotational_constraint() 185 | if constraint_return[0] != 0: 186 | for j in range(3): 187 | self.joints[self.rightLeg[i]][j] = constraint_return[j] 188 | 189 | mconstraint = Constraints(self.joints, self.leftLeg, i + 1, self.Theta[self.leftLeg[i + 1]], 190 | self.firstPos, 191 | self.constraint_type[self.leftLeg[i + 1]], 192 | self.orientation[self.leftLeg[i + 1]]) 193 | constraint_return = mconstraint.rotational_constraint() 194 | if constraint_return[0] != 0: 195 | for j in range(3): 196 | self.joints[self.leftLeg[i]][j] = constraint_return[j] 197 | 198 | def backward_arm(self, arm_index): 199 | # set root as initial position 200 | n = len(arm_index) 201 | for i in range(2, n): 202 | self.solve_for_distance(i - 1, i, arm_index) 203 | 204 | def backward_leg(self): 205 | # set root as initial position 206 | self.joints[self.rightLeg[0]] = self.firstPos[self.rightLeg[0]] 207 | self.joints[self.rightLeg[1]] = self.firstPos[self.rightLeg[1]] 208 | self.joints[self.leftLeg[0]] = self.firstPos[self.leftLeg[0]] 209 | self.joints[self.leftLeg[1]] = self.firstPos[self.leftLeg[1]] 210 | n = len(self.rightLeg) 211 | for i in range(2, n): 212 | # for Right leg 213 | self.solve_for_distance(i - 1, i, self.rightLeg) 214 | 215 | # for left leg 216 | self.solve_for_distance(i - 1, i, self.leftLeg) 217 | 218 | def solve(self): 219 | counter = 0 220 | sum_l = 0 221 | target_pos = self.laterality() 222 | ################################################################### 223 | # is target in reach or not 224 | if target_pos == "right": 225 | # arm length 226 | for i in range(len(self.rightArmIndex) - 1): 227 | sum_l = sum_l + util.distance(self.firstPos[self.rightArmIndex[i]], 228 | self.firstPos[self.rightArmIndex[i + 1]]) 229 | # chain length 230 | sum_l = sum_l + util.distance(self.firstPos[0], self.firstPos[self.rightLeg[len(self.rightLeg) - 1]]) 231 | # Leg length 232 | for i in range(len(self.rightLeg) - 1): 233 | sum_l = sum_l + util.distance(self.firstPos[self.rightLeg[i]], 234 | self.firstPos[self.rightLeg[i + 1]]) 235 | if sum_l < util.distance(self.firstPos[self.rightLeg[0]], self.target): 236 | print("target is out of reach!!!!!!!") 237 | return 238 | else: 239 | n = len(self.rightArmIndex) 240 | dif = util.distance(self.joints[self.rightArmIndex[n - 1]], self.target) 241 | 242 | else: 243 | # arm length 244 | for i in range(len(self.leftArmIndex) - 1): 245 | sum_l = sum_l + util.distance(self.firstPos[self.leftArmIndex[i]], 246 | self.firstPos[self.leftArmIndex[i + 1]]) 247 | # chain length 248 | sum_l = sum_l + util.distance(self.firstPos[0], self.firstPos[self.leftLeg[len(self.leftLeg) - 1]]) 249 | # Leg length 250 | for i in range(len(self.leftLeg) - 1): 251 | sum_l = sum_l + util.distance(self.firstPos[self.leftLeg[i]], self.firstPos[self.leftLeg[i + 1]]) 252 | if sum_l < util.distance(self.firstPos[self.leftLeg[0]], self.target): 253 | print("target is out of reach!!!!!!!") 254 | return 255 | else: 256 | n = len(self.leftArmIndex) 257 | dif = util.distance(self.joints[self.leftArmIndex[n - 1]], self.target) 258 | 259 | # target is in reach 260 | ########################################################################## 261 | 262 | while dif > self.solve_distance_threshold: 263 | if target_pos == "right": 264 | self.forward_arm(self.rightArmIndex, self.target, self.targetOrientation) 265 | else: 266 | self.forward_arm(self.leftArmIndex, self.target, self.targetOrientation) 267 | self.update_upper_chain_f(target_pos) 268 | self.update_lower_chain_f() 269 | self.forward_leg() 270 | 271 | self.backward_leg() 272 | self.update_lower_chain_b() 273 | self.update_upper_chain_b(target_pos) 274 | if target_pos == "right": 275 | self.backward_arm(self.rightArmIndex) 276 | else: 277 | self.backward_arm(self.leftArmIndex) 278 | 279 | if target_pos == "right": 280 | dif = util.distance(self.joints[self.rightArmIndex[n - 1]], self.target) 281 | else: 282 | dif = util.distance(self.joints[self.leftArmIndex[n - 1]], self.target) 283 | counter = counter + 1 284 | if counter > 10: 285 | break 286 | 287 | m_angle = AngleCalculator(OutputWriter().angle_writer(), self.joints,self.orientation) 288 | m_angle.calculate() 289 | draw_obj = Draw(self.joints, self.target, InputReader().initial_joints_position(), self.rightArmIndex, 290 | self.leftArmIndex, 291 | self.upperChain, 292 | self.lowerChain, self.rightLeg, self.leftLeg, self.neck, self.head) 293 | draw_obj.draw_final() 294 | -------------------------------------------------------------------------------- /h.py: -------------------------------------------------------------------------------- 1 | import math 2 | from pycg3d.cg3d_point import CG3dPoint 3 | from pycg3d.cg3d_vector import CG3dVector 4 | from pycg3d import utils 5 | import numpy as np 6 | 7 | 8 | class constraints: 9 | 10 | def __init__(self, joints, body_index, i, theta, length, constraint_type, orientation): 11 | self.x_global_axis =[1,0,0] 12 | self.y_global_axis = [0,1,0] 13 | self.joints = joints 14 | self.body_index = body_index 15 | self.i = i 16 | self.pi = 22 / 7 17 | self.theta = (self.pi / 180) * theta 18 | self.length = length 19 | self.constraint_type = constraint_type 20 | # the orientation of joint 21 | theta = math.acos(orientation[0]) * 2 22 | self.orientation_theta = theta 23 | self.joint_rotation_vector = self.joint_rotation_vector(orientation) 24 | 25 | def joint_rotation_vector(self, orientation): 26 | theta = math.acos(orientation[0]) * 2 27 | v1 = orientation[1]/math.sin(theta/2) 28 | v2 = orientation[2]/math.sin(theta/2) 29 | v3 = orientation[3]/math.sin(theta/2) 30 | return CG3dVector(v1,v2,v3) 31 | 32 | def rotate_about_axis(self,source, angle_degs, rotation_axis): 33 | sin_theta = math.sin(angle_degs * math.pi / 180) 34 | cos_theta = math.cos(angle_degs * math.pi / 180) 35 | one_minus_cos_theta = 1.0 - cos_theta 36 | 37 | xy_one = rotation_axis[0] * rotation_axis[1] * one_minus_cos_theta 38 | xz_one = rotation_axis[0] * rotation_axis[2] * one_minus_cos_theta 39 | yz_one = rotation_axis[1] * rotation_axis[2] * one_minus_cos_theta 40 | 41 | m00 = rotation_axis[0] * rotation_axis[0] * one_minus_cos_theta + cos_theta 42 | m01 = xy_one + rotation_axis[2] * sin_theta 43 | m02 = xz_one - rotation_axis[1] * sin_theta 44 | 45 | m10 = xy_one - rotation_axis[2] * sin_theta 46 | m11 = rotation_axis[1] * rotation_axis[1] * one_minus_cos_theta + cos_theta 47 | m12 = yz_one + rotation_axis[0] * sin_theta 48 | 49 | m20 = xz_one + rotation_axis[1] * sin_theta 50 | m21 = yz_one - rotation_axis[0] * sin_theta 51 | m22 = rotation_axis[2] * rotation_axis[2] * one_minus_cos_theta + cos_theta 52 | 53 | rotation_matrix = [[m00, m01, m02], 54 | [m10, m11, m12], 55 | [m20, m21, m22]] 56 | result_times = [] 57 | for i in range(0, 3): 58 | result_row = 0 59 | for j in range(0, 3): 60 | result_row += rotation_matrix[i][j] * source[j] 61 | result_times.append(result_row) 62 | result_times = CG3dVector(result_times[0],result_times[1],result_times[2]) 63 | return result_times 64 | 65 | def rotational_constraint(self): 66 | p_i = CG3dPoint(self.joints[self.body_index[self.i]][0], self.joints[self.body_index[self.i]][1], 67 | self.joints[self.body_index[self.i]][2]) 68 | p_before = CG3dPoint(self.joints[self.body_index[self.i - 1]][0], self.joints[self.body_index[self.i - 1]][1], 69 | self.joints[self.body_index[self.i - 1]][2]) 70 | p_next = CG3dPoint(self.joints[self.body_index[self.i + 1]][0], self.joints[self.body_index[self.i + 1]][1], 71 | self.joints[self.body_index[self.i + 1]][2]) 72 | 73 | v_i_next = CG3dVector(p_next[0] - p_i[0], p_next[1] - p_i[1], p_next[2] - p_i[2]) 74 | v_before_i = CG3dVector(p_i[0] - p_before[0], p_i[1] - p_before[1], p_i[2] - p_before[2]) 75 | 76 | dot_prod = v_i_next * v_before_i 77 | s = dot_prod 78 | 79 | # a unit vector of a line passing from p(i+1) and P(i) 80 | l_next_i = utils.distance(p_next, p_i) 81 | unit_vec_next_i = CG3dVector((p_i[0] - p_next[0]) / l_next_i, 82 | (p_i[1] - p_next[1]) / l_next_i, 83 | (p_i[2] - p_next[2]) / l_next_i) 84 | 85 | # the center of cone 86 | o = CG3dPoint(p_i[0] + unit_vec_next_i[0] * s, p_i[1] + unit_vec_next_i[1] * s, 87 | p_i[2] + unit_vec_next_i[2] * s) 88 | 89 | if (self.constraint_type == "hinge"): 90 | # normal plane vector of p(next), p(i), p(before) 91 | normal_plane = v_i_next ^ v_before_i 92 | l = math.sqrt(normal_plane * normal_plane) 93 | uv_normal_plane = CG3dVector(normal_plane[0] / l, normal_plane[1] / l, normal_plane[2] / l) 94 | # a vector from p_i to o 95 | l_i__o = utils.distance(p_i, o) 96 | unit_vec_i_o = CG3dVector((o[0] - p_i[0]) / l_i__o, 97 | (o[1] - p_i[1]) / l_i__o, 98 | (o[2] - p_i[2]) / l_i__o) 99 | 100 | axis_normal_plane = [uv_normal_plane[0], uv_normal_plane[1], uv_normal_plane[2]] 101 | unit_vec_i_o_array = np.array([[unit_vec_i_o[0]], [unit_vec_i_o[1]], [unit_vec_i_o[2]]]) 102 | # rotating p(i)-o C.C.W to find flexion constraint(left of pi_o) 103 | p_down = np.dot(self.rotation_matrix(axis_normal_plane, self.theta[1]), unit_vec_i_o_array) 104 | p_down = CG3dVector(p_down[0, 0], p_down[1, 0], p_down[2, 0]) 105 | # rotating p(i)-o C.W to find extension constraint(right of pi_o ) 106 | p_up = np.dot(self.rotation_matrix(axis_normal_plane, -self.theta[3]), unit_vec_i_o_array) 107 | p_up = CG3dVector(p_up[0, 0], p_up[1, 0], p_up[2, 0]) 108 | 109 | l_before_i = utils.distance(p_i, p_before) 110 | uv_i_before = CG3dVector((p_before[0] - p_i[0]) / l_before_i, (p_before[1] - p_i[1]) / l_before_i, 111 | (p_before[2] - p_i[2]) / l_before_i) 112 | 113 | if (uv_i_before ^ unit_vec_i_o) * (uv_normal_plane) > 0: 114 | # means it is upper side of pi_o 115 | if self.theta[3] != 0: 116 | # angle between vec(i_before) and vec(i_o) 117 | angle = math.acos(uv_i_before * unit_vec_i_o) 118 | if angle <= self.theta[3]: 119 | return CG3dPoint(0, 0, 0) 120 | else: 121 | p_nearest_point = p_i + l_before_i * p_up 122 | return p_nearest_point 123 | else: 124 | p_nearest_point = CG3dPoint(p_i[0] + unit_vec_i_o[0] * l_before_i, 125 | p_i[1] + unit_vec_i_o[1] * l_before_i, 126 | p_i[2] + unit_vec_i_o[2] * l_before_i) 127 | return p_nearest_point 128 | else: 129 | # means it is down side of pi_o 130 | if self.theta[1] != 0: 131 | # angle between vec(i_before) and vec(i_o) 132 | angle = math.acos(uv_i_before * unit_vec_i_o) 133 | if angle <= self.theta[1]: 134 | return CG3dPoint(0, 0, 0) 135 | else: 136 | p_nearest_point = p_i + l_before_i * p_down 137 | return p_nearest_point 138 | else: 139 | p_nearest_point = CG3dPoint(p_i[0] + unit_vec_i_o[0] * l_before_i, 140 | p_i[1] + unit_vec_i_o[1] * l_before_i, 141 | p_i[2] + unit_vec_i_o[2] * l_before_i) 142 | return p_nearest_point 143 | if self.constraint_type == "ballAndSocket": 144 | # Semi ellipsoidal parameter qi (1,2,3,4) 145 | q1 = round(s * math.tan(self.theta[0]), 3) 146 | q2 = round(s * math.tan(self.theta[1]), 3) 147 | q3 = round(s * math.tan(self.theta[2]), 3) 148 | q4 = round(s * math.tan(self.theta[3]), 3) 149 | 150 | # change the coordinate to cross section of cone and calculating the (i-1)th position in it 151 | l_o_before = utils.distance(o, p_before) 152 | v_o_before = CG3dVector(p_before[0] - o[0], p_before[1] - o[1], p_before[2] - o[2]) 153 | uv_o_before = CG3dVector(v_o_before[0]/l_o_before,v_o_before[1]/l_o_before,v_o_before[2]/l_o_before) 154 | 155 | # joint reference plane coordinate axis: 156 | rotation_axis = self.joint_rotation_vector 157 | x_reference_axis = self.normalization(self.rotate_about_axis(self.x_global_axis,self.orientation_theta,rotation_axis)) 158 | y_reference_axis = self.normalization(self.rotate_about_axis(self.y_global_axis, self.orientation_theta, rotation_axis)) 159 | 160 | x_t = v_o_before*x_reference_axis 161 | y_t = v_o_before*y_reference_axis 162 | 163 | if x_t>0 and y_t>=0: 164 | sector = 1 165 | elif x_t<=0 and y_t>0: 166 | sector = 2 167 | elif x_t<0 and y_t<=0: 168 | sector = 3 169 | elif x_t>=0 and y_t<0: 170 | sector = 4 171 | 172 | # checking that the target point is in ellipsoidal shape 173 | inbound = 0 174 | if x_t==0 and y_t==0: 175 | inbound = 1 176 | p_prime = CG3dPoint(0, 0, 0) 177 | return p_prime 178 | 179 | if round(((x_t ** 2) / (q2 ** 2) + (y_t ** 2) / (q3 ** 2))) <= 1 and sector == 1: 180 | inbound = 1 181 | p_prime = CG3dPoint(0, 0, 0) 182 | return p_prime 183 | elif round(((x_t ** 2) / (q2 ** 2) + (y_t ** 2) / (q1 ** 2))) <= 1 and sector == 2: 184 | inbound = 1 185 | p_prime = CG3dPoint(0, 0, 0) 186 | return p_prime 187 | elif round(((x_t ** 2) / (q4 ** 2) + (y_t ** 2) / (q1 ** 2))) <= 1 and sector == 3: 188 | inbound = 1 189 | p_prime = CG3dPoint(0, 0, 0) 190 | return p_prime 191 | elif round(((x_t ** 2) / (q4 ** 2) + (y_t ** 2) / (q3 ** 2))) <= 1 and sector == 4: 192 | inbound = 1 193 | p_prime = CG3dPoint(0, 0, 0) 194 | return p_prime 195 | 196 | # if it is out bound of the ellipsoidal shape we should find the nearest point on ellipsoidal shape 197 | if inbound == 0 and sector == 1: 198 | if y_t != 0: 199 | result = self.find_nearest_2(x_t, y_t ,q2, q3) 200 | x_nearest_point = result[0] 201 | y_nearest_point = result[1] 202 | else: 203 | x_nearest_point = q2 204 | y_nearest_point = 0 205 | elif inbound == 0 and sector == 2: 206 | if x_t != 0: 207 | result = self.find_nearest_2(x_t, y_t ,q2, q1) 208 | x_nearest_point = result[0] 209 | y_nearest_point = result[1] 210 | else: 211 | x_nearest_point = 0 212 | y_nearest_point = -q1 213 | elif inbound == 0 and sector == 3: 214 | if y_t != 0: 215 | result = self.find_nearest_2(x_t, y_t ,q4, q1) 216 | x_nearest_point = result[0] 217 | y_nearest_point = result[1] 218 | else: 219 | x_nearest_point = -q4 220 | y_nearest_point = 0 221 | elif inbound == 0 and sector == 4: 222 | if x_t != 0: 223 | result = self.find_nearest_2(x_t, y_t ,q4, q3) 224 | x_nearest_point = result[0] 225 | y_nearest_point = result[1] 226 | else: 227 | x_nearest_point = 0 228 | y_nearest_point = q3 229 | 230 | 231 | l_o_nearest_point = math.sqrt(x_nearest_point**2 +y_nearest_point**2) 232 | nearest_point_in_global = CG3dPoint(o[0]+uv_o_before[0]*l_o_nearest_point, 233 | o[1]+uv_o_before[1]*l_o_nearest_point, 234 | o[2]+uv_o_before[2]*l_o_nearest_point) 235 | # finding nearest point global coordinate in 3d 236 | # l_o_before = math.sqrt(x_t ** 2 + y_t ** 2) 237 | # l_o_nearest_point = math.sqrt(x_nearest_point ** 2 + y_nearest_point ** 2) 238 | # l_nearest_before = math.sqrt((x_t - x_nearest_point) ** 2 + (y_nearest_point - y_t) ** 2) 239 | # 240 | # # rotation angle between vector from o to p_before and o to p_nearest point 241 | # rot_angle = (math.acos(round((l_nearest_before ** 2 - l_o_before ** 2 - l_o_nearest_point ** 2) / ( 242 | # -2 * l_o_before * l_o_nearest_point)))) 243 | # 244 | # # rotating the vector from o to p_before around vector from p_next to o 245 | # 246 | # # a vector from o to p_before 247 | # unit_vec_o_before = CG3dVector((p_before[0] - o[0]) / l_o_before, 248 | # (p_before[1] - o[1]) / l_o_before, 249 | # (p_before[2] - o[2]) / l_o_before) 250 | # unit_vec_normal_plane = [(p_i[0] - p_next[0]) / l_next_i, 251 | # (p_i[1] - p_next[1]) / l_next_i, 252 | # (p_i[2] - p_next[2]) / l_next_i] 253 | # 254 | # if rot_angle == 0: 255 | # p_nearest_point_in_global = CG3dPoint(o[0] + l_o_nearest_point * unit_vec_o_before[0], 256 | # o[1] + l_o_nearest_point * unit_vec_o_before[1], 257 | # o[2] + l_o_nearest_point * unit_vec_o_before[2]) 258 | # return p_nearest_point_in_global 259 | # else: 260 | # # to find out nearest point is on left or right side of target point 261 | # orient_near_to_target = x_t * y_nearest_point - x_nearest_point * y_t 262 | # unit_vec_o_before = np.array([[unit_vec_o_before[0]], 263 | # [unit_vec_o_before[1]], 264 | # [unit_vec_o_before[2]]]) 265 | # if orient_near_to_target * unit_vec_next_i[2] > 0: 266 | # # to find nearest point should rotate the uv(o-target) in C.C.W 267 | # uv_o_nearest_point = np.dot(self.rotation_matrix(unit_vec_normal_plane, rot_angle), 268 | # unit_vec_o_before) 269 | # else: 270 | # uv_o_nearest_point = np.dot(self.rotation_matrix(unit_vec_normal_plane, -rot_angle), 271 | # unit_vec_o_before) 272 | # 273 | # p_nearest_point_in_global = CG3dPoint(o[0] + l_o_nearest_point * uv_o_nearest_point[0], 274 | # o[1] + l_o_nearest_point * uv_o_nearest_point[1], 275 | # o[2] + l_o_nearest_point * uv_o_nearest_point[2]) 276 | 277 | return nearest_point_in_global 278 | 279 | def find_nearest_2(self,x_t,y_t,a,b): 280 | m = x_t/y_t 281 | y_nearest_point = math.sqrt(abs(1- (m**2/a**2+1/b**2))) 282 | x_nearest_point = m*y_nearest_point 283 | return [x_nearest_point,y_nearest_point] 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | def rotation_matrix(self, axis, theta): 301 | """ 302 | Return the rotation matrix associated with counterclockwise rotation about 303 | the given axis by theta radians. 304 | """ 305 | axis = np.asarray(axis) 306 | axis = axis / math.sqrt(np.dot(axis, axis)) 307 | a = math.cos(theta / 2.0) 308 | b, c, d = -axis * math.sin(theta / 2.0) 309 | aa, bb, cc, dd = a * a, b * b, c * c, d * d 310 | bc, ad, ac, ab, bd, cd = b * c, a * d, a * c, a * b, b * d, c * d 311 | return np.array([[aa + bb - cc - dd, 2 * (bc + ad), 2 * (bd - ac)], 312 | [2 * (bc - ad), aa + cc - bb - dd, 2 * (cd + ab)], 313 | [2 * (bd + ac), 2 * (cd - ab), aa + dd - bb - cc]]) 314 | 315 | def find_initial_point(self, a, b, sector, x_t, y_t): 316 | x_k1 = x_t * a * b / math.sqrt(b ** 2 * x_t ** 2 + a ** 2 * y_t ** 2) 317 | y_k1 = y_t * a * b / math.sqrt(b ** 2 * x_t ** 2 + a ** 2 * y_t ** 2) 318 | if sector == 1 or sector == 2: 319 | sign_y = 1 320 | elif sector == 3 or sector == 4: 321 | sign_y = -1 322 | if sector == 1 or sector == 4: 323 | sign_x = 1 324 | elif sector == 3 or sector == 2: 325 | sign_x = -1 326 | 327 | if abs(x_t) < a: 328 | x_k2 = x_t 329 | y_k2 = sign_y * (b / a) * math.sqrt(a ** 2 - x_t ** 2) 330 | else: 331 | x_k2 = a * sign_x * a 332 | y_k2 = 0 333 | 334 | x0 = 0.5 * (x_k1 + x_k2) 335 | y0 = 0.5 * (y_k1 + y_k2) 336 | 337 | initial = np.array([x0, y0]) 338 | return initial 339 | 340 | def q_matrix(self, x, y, a, b, x_t, y_t): 341 | Q = np.matrix( 342 | [[b ** 2 * x, a ** 2 * y], [(a ** 2 - b ** 2) * y + b ** 2 * y_t, (a ** 2 - b ** 2) * x - a ** 2 * x_t]]) 343 | return Q 344 | 345 | def find_delta(self, x, y, a, b, x_t, y_t): 346 | f1 = (a ** 2 * y ** 2 + b ** 2 * x ** 2 - a ** 2 * b ** 2) / 2 347 | f2 = b ** 2 * x * (y_t - y) - a ** 2 * y * (x_t - x) 348 | f = np.matrix([[f1], [f2]]) 349 | Q = self.q_matrix(x, y, a, b, x_t, y_t) 350 | delta = -1 * (np.linalg.inv(Q) * f) 351 | 352 | result = np.array([delta[0, 0], delta[1, 0]]) 353 | return result 354 | 355 | def find_next_point(self, x, y, a, b, x_t, y_t): 356 | delta = self.find_delta(x, y, a, b, x_t, y_t) 357 | x = x + delta[0] 358 | y = y + delta[1] 359 | result = np.array([x, y]) 360 | return result 361 | 362 | def find_nearest_point(self, a, b, sector, x_t, y_t): 363 | initial = self.find_initial_point(a, b, sector, x_t, y_t) 364 | x_initial = initial[0] 365 | y_initial = initial[1] 366 | x = x_initial 367 | y = y_initial 368 | thresh = 0.001 369 | result = self.find_next_point(x, y, a, b, x_t, y_t) 370 | counter =0 371 | while result[0] > thresh or result[1] > thresh and counter<1000: 372 | x = result[0] 373 | y = result[1] 374 | result = self.find_next_point(x, y, a, b, x_t, y_t) 375 | counter+=1 376 | x_nearest_point = result[0] 377 | y_nearest_point = result[1] 378 | return np.array([x_nearest_point, y_nearest_point]) 379 | 380 | def normalization(self,vector): 381 | l = math.sqrt(vector[0] ** 2 + vector[1] ** 2 + vector[2] ** 2) 382 | if l == 0: 383 | l += 0.001 384 | normal_vector = CG3dVector(vector[0] / l, vector[1] / l, vector[2] / l) 385 | return normal_vector --------------------------------------------------------------------------------