fabrik 算法的python 实现

IK 运动动力学逆运算的快速，简单迭代算法FABRIK 介绍了算法，现在介绍python 实现算法。

这是一个无约束单链fabrik 算法实现。这个也是有约束和多链的算法基础。

代码放置在

https://github.com/liwenz/Fabrik

这里主要介绍使用方法，代码你就直接看，也不复杂。

 Fabrik 算法的一个非常小而灵活的实现。 适用于动画、机器人或其他优化问题。 这个模块是用python编写的。

fabrik.py : fabrik 类，它解决不受约束的 ik 单链。它可以是 2D、3D 或 nD

init：输入 ik init 位置和 marginOfError。

distance：计算两点之间的距离

compute：输入新的终点并获得 fabrik 结果。

ComputeDiff：输入新端点的差值并得到 fabrik 结果。

calAngle：获取当前角度。

isReachable：检查结果是否可能。

test.py：这是一个演示。 用两种方式构造点数组，然后 fabrik ，打印结果

dog.py 机器狗的单腿计算演示获取结果角度数组

dog4.py 机器狗的4条腿计算demo得到结果数组

这里附上fabrik.py ，其他可到github.com/liwenz/fabrik 查看下载

# -*- coding: utf-8 -*-
"""
Created on Wed Sep 22 13:26:39 2021
@author: zeng_
"""

import numpy as np
import math

class FabrikSolver:
    """ 
        An inverse kinematics solver in Fabrik Algorithm.
    """
    def __init__(self, _points, _marginOfError=0.01):
        self.points=np.copy(_points)
        self.marginOfError=_marginOfError
        self.ndim=self.points.ndim
        self.number=int(self.points.size/self.ndim)
        self.length=np.zeros(self.number-1,dtype=float)
        self.angle=np.zeros(self.number-1,dtype=float)
        #print('ndim= ',self.ndim)
        #print('number= ',self.number)

        self.maxLenth=0.0
        for i in range(self.number-1):
            self.length[i] = self.distance(self.points[i],self.points[i+1])
            self.maxLenth+=self.length[i]
            #print(self.length[i])
        #print('maxlenth= ',self.maxLenth)
    def distance(self,a,b):
        dist=0.0
        for i in range(self.ndim):
            dist=dist+(a[i]-b[i])*(a[i]-b[i])
        dist=math.sqrt(dist)
        return dist
    def CalAngle(self):
        for i in range(self.number-1):
            p=self.points[i+1]-self.points[i]
            self.angle[i]=math.atan2(p[1], p[0])*180/math.pi
        return self.angle



    def Backword(self):
        #print("backword")
        self.points[self.number-1]=self.target
        for i in range(self.number-1):
            q=self.number-1-i
            l=self.distance(self.points[q], self.points[q-1])
            r=self.length[q-1]/l
            #print(i,q,l,r)
            self.points[q-1]=self.points[q-1]*r+self.points[q]*(1-r)
        #print(self.points)
        
    def Forword(self):
        self.points[0]=np.zeros(self.ndim,dtype=float)
        #print("forword")
        for i in range(self.number-1):
            q=i+1
            l=self.distance(self.points[q], self.points[q-1])
            r=self.length[q-1]/l
            #print(i,q,l,r)
            self.points[q]=self.points[q]*r+self.points[q-1]*(1-r)
        #print(self.points)
    def Compute(self,target):
        self.target=target
        print("target= ",self.target)
        if(not(self.isReachable())):
            print("is Not Reachable")
            return 100
        deviate=10000
        while(deviate>self.marginOfError):
            self.Backword()
            self.Forword()
            deviate=self.distance(self.points[self.number-1],self.target)
            #print('deviate= ',deviate)
        return deviate
    def ComputeDiff(self,targetDiff):
        return self.Compute(targetDiff+self.points[self.number-1])
    
    def isReachable(self):

        return (self.maxLenth>=self.distance(self.points[0],self.target))