自动做微网站,南京环力建设有限公司网站,WordPress签到打卡,怎样在网站做友情链接目录 一、前言二、fabrik 算法三、python实现结论PS.扩展阅读ps1.六自由度机器人相关文章资源ps2.四轴机器相关文章资源ps3.移动小车相关文章资源ps3.wifi小车控制相关文章资源 一、前言
我们用blender等3D动画软件时#xff0c;会用到骨骼的动画#xff0c;通过逆向IK动力学… 目录 一、前言二、fabrik 算法三、python实现结论PS.扩展阅读ps1.六自由度机器人相关文章资源ps2.四轴机器相关文章资源ps3.移动小车相关文章资源ps3.wifi小车控制相关文章资源 一、前言
我们用blender等3D动画软件时会用到骨骼的动画通过逆向IK动力学可以实现控制少量点就能控制一个复杂的骨架运动。这种IK动力学几乎是实时的非常的高效。这种IK动力学算法的代表是fabrik 算法该算法应被用于UE虚幻引擎、Unity等3D软件中。大至效果是可以实现骨架的目标跟随而且几乎是“实时”的 这种感觉不就是机械臂的虚拟拖拽吗是否可以给机械臂的逆解或者是虚拟化示教带来一些启发是一个有意思的应用。本篇先来初步研究一下fabrik 算法。
二、fabrik 算法
该算法在文章 FABRIK: A fast, iterative solver for the Inverse Kinematics problem中有详细说明FABRIK算法是一种用于解决逆运动学问题的启发式方法。它通过迭代地调整关节链使末端执行器逐渐接近目标位置。与传统方法相比FABRIK算法不需要使用旋转角度或矩阵而是通过在线段上定位点来找到每个关节的位置这使得它在计算上更加高效并且能够产生视觉上现实的关节姿势。
三、python实现
网上已经有很多实现的python算法这里主要是利用实现的3D算法实现在matplot中的IK即任意点击3D坐标点实现IK骨架的末端移动到目标点就像开始的blender一样。 首先我们需要导入一些必要的Python库包括NumPy、Math、Matplotlib等用于数学运算和图形绘制。
import numpy as np
import math
import matplotlib as mpl
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import axes3d
import re单位向量函数 定义了一个函数 unitVector它接受一个向量作为输入并返回该向量的单位向量。
def unitVector(vector):return vector / np.linalg.norm(vector)Segment3D 类用于存储逆运动学链的一部分。它接受参考点坐标、段长度和初始角度作为参数并计算出该段的新坐标。
class Segment3D:def __init__(self, referenceX, referenceY, referenceZ, length, zAngle, yAngle):self.zAngle zAngleself.yAngle yAngleself.length lengthdeltaX math.cos(math.radians(zAngle)) * lengthdeltaY math.sin(math.radians(zAngle)) * lengthdeltaZ math.sin(math.radians(yAngle)) * lengthnewX referenceX deltaXnewY referenceY deltaYnewZ referenceZ deltaZself.point np.array([newX, newY, newZ])FabrikSolver3D 类是FABRIK算法的核心它初始化了一个3D逆运动学求解器并提供了添加段、检查可达性、迭代求解和绘图等功能。 class FabrikSolver3D: An inverse kinematics solver in 3D. Uses the Fabrik Algorithm.def __init__(self, baseX0, baseY0, baseZ0, marginOfError0.01):# Create the base of the chain.self.basePoint np.array([baseX, baseY, baseZ])# Initialize empty segment array - [].self.segments []# Initialize length of the chain - 0.self.armLength 0# Initialize the margin of error.self.marginOfError marginOfErrorself.targetpointNoneself.fig plt.figure()self.ax1 self.fig.add_subplot(111, projection3d)self.mousepNonedef addSegment(self, length, zAngle, yAngle):if len(self.segments) 0:segment Segment3D(self.segments[-1].point[0], self.segments[-1].point[1], self.segments[-1].point[2], length, zAngle self.segments[-1].zAngle, self.segments[-1].yAngle yAngle)else:# Maak een segment van de vector beginpoint, lengte en hoek.segment Segment3D(self.basePoint[0], self.basePoint[1], self.basePoint[2], length, zAngle, yAngle)# Voeg lengte toe aan de totale armlengte.self.armLength segment.length# Voeg de nieuwe segment toe aan de list.self.segments.append(segment)def isReachable(self, targetX, targetY, targetZ):if np.linalg.norm(self.basePoint - np.array([targetX, targetY, targetZ])) self.armLength:return Truereturn Falsedef inMarginOfError(self, targetX, targetY, targetZ):if np.linalg.norm(self.segments[-1].point - np.array([targetX, targetY, targetZ])) self.marginOfError:return Truereturn False def iterate(self, targetX, targetY, targetZ):target np.array([targetX, targetY, targetZ])# Backwards.for i in range(len(self.segments) - 1, 0, -1):if i len(self.segments) - 1:self.segments[i-1].point (unitVector(self.segments[i-1].point - target) * self.segments[i].length) targetelse:self.segments[i-1].point (unitVector(self.segments[i-1].point - self.segments[i].point) * self.segments[i].length) self.segments[i].point# Forwards.for i in range(len(self.segments)):if i 0:self.segments[i].point (unitVector(self.segments[i].point - self.basePoint) * self.segments[i].length) self.basePointelif i len(self.segments) - 1:self.segments[i].point (unitVector(self.segments[i-1].point - target) * self.segments[i].length * -1) self.segments[i-1].pointelse:self.segments[i].point (unitVector(self.segments[i].point - self.segments[i-1].point) * self.segments[i].length) self.segments[i-1].pointdef compute(self, targetX, targetY, targetZ):if self.isReachable(targetX, targetY, targetZ):while not self.inMarginOfError(targetX, targetY, targetZ):self.iterate(targetX, targetY, targetZ) self.targetpoint[targetX, targetY, targetZ]else:print(Target not reachable.)sys.exit()def plot(self, saveFalse, namegraph):self.ax1.clear() # 清除当前轴ax1self.ax1# Plot arm.for i, segment in enumerate(self.segments):#ax1.scatter(segment.point[2], segment.point[0], segment.point[1], cb)ax1.scatter(segment.point[0], segment.point[1], segment.point[2], cb)if i 0: # Connect to the previous segmentax1.plot([self.segments[i-1].point[0], segment.point[0]],[self.segments[i-1].point[1], segment.point[1]],[self.segments[i-1].point[2], segment.point[2]], b-)ax1.plot([self.segments[i-1].point[2], segment.point[2]],[self.segments[i-1].point[0], segment.point[0]],[self.segments[i-1].point[1], segment.point[1]], b-)# Connect the last segment to the base pointax1.plot([self.basePoint[2], self.segments[0].point[2]],[self.basePoint[0], self.segments[0].point[0]],[self.basePoint[1], self.segments[0].point[1]], b-)ax1.plot([self.basePoint[0], self.segments[0].point[0]],[self.basePoint[1], self.segments[0].point[1]],[self.basePoint[2], self.segments[0].point[2]], b-)# Start point#ax1.scatter(self.basePoint[2], self.basePoint[0], self.basePoint[1], cg)ax1.scatter(self.basePoint[0], self.basePoint[1], self.basePoint[2], cg)ax1.scatter(self.targetpoint[0],self.targetpoint[1],self.targetpoint[2], cr,alpha0.6, s100)ax1.set_ylabel(y-axis)ax1.set_zlabel(z-axis)ax1.set_xlabel(x-axis)# Set the view angle so that the z-axis is pointing upwardsax1.view_init(elev45., azim45.)plt.pause(0.01)def extract_coordinates(self,s):# 使用正则表达式匹配x, y, z的值# 注意负号前面加上了反斜杠进行转义# 使用正则表达式提取实数值包括负号pattern r−?\d\.\dmatches re.findall(pattern, s)values [float(match.replace(−, -)) for match in matches]x, y, zvalues return x, y, zdef show(self):self.plot()self.fig.canvas.mpl_connect(button_press_event, self.on_click) # 连接点击事件self.fig.canvas.mpl_connect(motion_notify_event, self.on_motion) plt.show()def on_motion(self,event):if event.inaxes is not None:x, y event.xdata, event.ydatasting self.ax1.format_coord(x, y) x,y,zself.extract_coordinates(sting)#print(x,y,z) self.mousep(x,y,z)def on_click(self, event):# 检查点击事件是否在坐标轴内if event.inaxes is not None:print(fClicked on axis {event.inaxes})# 触发你的函数print(self.mousep)if not type(self.mousep) type(None): x,y,zself.mousepprint(compute)self.compute(x, y, z)self.plot()# 获取点击的坐标值#self.compute(x, y, z)# 重绘图形#可以通过如下步骤实现逆解 添加关节段通过addSegment方法我们可以为机械臂添加多个关节段每个关节段都有自己的长度和初始角度。 检查可达性isReachable方法检查目标位置是否在机械臂的可达范围内。 迭代计算iterate方法执行一次FABRIK算法迭代调整关节位置以接近目标。 绘图显示plot方法用于在3D空间中绘制机械臂的当前状态show方法则显示最终的图形界面并允许用户通过点击来选择目标位置。 事件处理on_click和on_motion方法用于处理用户的点击和鼠标移动事件以便动态地调整目标位置。 if __name__ __main__: arm FabrikSolver3D() arm.addSegment(0, 0, 0)arm.addSegment(50, 0, 0)arm.addSegment(50, 0, 0)arm.addSegment(50, 0, 0)arm.addSegment(50, 0, 0)arm.compute(50, 50, 100) arm.show()结论
通过这个Python实现我们可以看到FABRIK算法在解决3D逆运动学问题中的强大能力。它不仅能够快速找到解决方案还能够实时响应用户的交互这在模拟和实际应用中都是非常有价值的。接下来我们尝试丰富这个算法在各关节添加约束并应用到6轴机械臂的IK计算中看看能否获得预期效果。
[------------本篇完-------------]
PS.扩展阅读
————————————————————————————————————————
对于python机器人编程感兴趣的小伙伴可以进入如下链接阅读相关咨询
ps1.六自由度机器人相关文章资源
(1) 对六自由度机械臂的运动控制及python实现附源码)
(2) N轴机械臂的MDH正向建模及python算法
ps2.四轴机器相关文章资源
(1) 文章python机器人编程——用python实现一个写字机器人 2python机器人实战——0到1创建一个自动是色块机器人项目-CSDN直播
(3)博文《我从0开始搭建了一个色块自动抓取机器人并实现了大模型的接入和语音控制-(上基础篇)》的vrep基础环境 (3)博文《我从0开始搭建了一个色块自动抓取机器人并实现了大模型的接入和语音控制-(上基础篇)》的vrep基础环境 (4)实现了语音输入大模型指令解析机器视觉机械臂流程打通
ps3.移动小车相关文章资源
1python做了一个极简的栅格地图行走机器人到底能干啥[第五弹]——解锁蒙特卡洛定位功能-CSDN博客 (2) 对应python资源源码地址
(3)python机器人编程——差速AGV机器、基于视觉和预测控制的循迹、自动行驶上篇_agv编程-CSDN博客 (4)python机器人编程——差速AGV机器、基于视觉和预测控制的循迹、自动行驶下篇_agv路线规划原则python-CSDN博客 对应python及仿真环境资源源码链接
ps3.wifi小车控制相关文章资源
web端配套资源源代码已经上传(竖屏版)下载地址 仿真配套资源已经上传下载地址 web端配套资源源代码已经上传(横屏版)下载地址
在这里插入代码片
