将圆柱体看作机器人的加速和减速运动机
申明:本人今年博一(地点英国),项目是与机器人有关的内容。第三次导师见面,导师让我做一个mini-experiment,用到“PyBullet”。
系列文章目录:
上一篇:
PyBullet (二) 机器人手臂的简单移动
下一篇:
PyBullet (四) 将圆柱体看作机器人,推动目标,让目标按照输入的路径在可移动障碍物中移动
虚拟机:VMware Workstation 16.x Pro
系统:Ubuntu20.04.1(官方下载链接)
PyBullet:(官方链接)(官方指南)
1. 整体思路
将圆柱体看作是机器人来进行移动,对圆柱体施加力推动圆柱体的移动,
2. 代码解析
2.1 代码分布
2.1.1 库
import pybullet as p
import time
import pybullet_data
最基础的导入包啊,库啊
2.1.2 连接物理模拟
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
导入PyBullet模块后,首先要做的是 "连接 "物理模拟。PyBullet是围绕客户端-服务器驱动的API设计的,客户端发送命令,物理服务器返回状态。PyBullet有一些内置的物理服务器。DIRECT和GUI。GUI和DIRECT连接都会在与PyBullet相同的过程中执行物理模拟和渲染。
2.1.3 重力设置
p.setGravity(0,0,-9.81)
设置重力系数分别是:x方向(+:x轴正半轴;-:x轴负半轴);y方向(+:y轴正半轴;-:y轴负半轴);z方向(+:z轴正半轴;-:z轴负半轴)。
2.1.4 设置基础参数
cylinderStartPos = [0,0,0.3]#圆柱体的初始位置
cylinderStartOrientation = p.getQuaternionFromEuler([0,0,0])#(圆柱体)这里的参数会转换成一个四元数,可以理解成能够控制模型在空间中绕x,y,z轴旋转的参数。(参数是角度。e.g. [3.14,0,0] == [pai,0,0];[1.57,0,0] == [pai/2,0,0]。参数的正负表示旋转的不同方向。)
2.1.5 模型加载
planeId = p.loadURDF("plane.urdf")
cylinderId = p.loadURDF("/home/xzs/PyBullet_Practice/bullet3-master/data/cylinder1.urdf",cylinderStartPos,cylinderStartOrientation)
cylinder1.urdf:
<?xml version="1.0"?>
<robot name="myfirst">
<material name="blue">
<color rgba="0 1 1 1"/>
</material>
<link name="base_link">
<visual>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<cylinder length="0.6" radius="0.2"/>
</geometry>
</collision>
<inertial>
<mass value="1"/>
<inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
</inertial>
</link>
</robot>
具体参数细节请访问PyBullet (二) 机器人手臂的简单移动中的“2.14模型加载(一)”
2.1.6 模型设置
p.changeDynamics(planeId,-1,lateralFriction = 0,spinningFriction = 0,rollingFriction = 0)#这里是为了设置“plane”的参数,让“plane”变成理想平面(无摩擦力)
changeDynamics
您可以使用changeDynamics改变质量、摩擦和归位系数等属性。
必需/可选参数 | 参数名字 | 参数类型 | 介绍 |
---|---|---|---|
必需 | objectUniqueId | int | 对象的唯一id,由加载方法返回。 |
必需 | linkIndex | int | 链接指数或-1为基准。 |
可选 | mass | double | 改变链接的质量(或链接指数-1的基础)。 |
可选 | lateralFriction | double | 横向(线性)接触摩擦。 |
可选 | spinningFriction | double | 围绕接触法线的扭转摩擦。 |
可选 | rollingFriction | double | 与接触法线正交的扭转摩擦力(保持这个值非常接近于零,否则模拟会变得非常不真实)。 |
还有更多参数,这里仅列出了一些相对常用的,如果想了解更多参数请私戳旁边的链接(当然我会慢慢加上来,明天有和导师的会议时间比较紧,我还要写report)(官方指南)
2.1.7 移动开始
for k in range(3):#移动三个阶段
for i in range (480):#控制施加力的时间
p.applyExternalForce(cylinderId,-1,[1,0,0],[0,0,0.3],p.WORLD_FRAME)#施加延x轴正方向的力
p.stepSimulation()
time.sleep(1./240.)
for j in range (480):#控制施加力的时间
p.applyExternalForce(cylinderId,-1,[-1,0,0],[0,0,0.3],p.WORLD_FRAME)#施加延x轴负方向的力(为了减速)
p.stepSimulation()
time.sleep(1./240.)
applyExternalForce
您可以使用applyExternalForce和applyExternalTorque将力或扭矩施加到一个体上。请注意,只有在使用stepSimulation显式步进模拟时,该方法才会起作用,换句话说:setRealTimeSimulation(0)。在每次模拟步骤后,外力都会被清零。如果你使用’setRealTimeSimulation(1),applyExternalForce/Torque将有未定义的行为(要么是0,要么是1,要么是多个力/扭矩应用)。
必需/可选参数 | 参数名字 | 参数类型 | 介绍 |
---|---|---|---|
必需 | objectUniqueId | int | 对象的唯一id,由加载方法返回。 |
必需 | linkIndex | int | 链接索引或基数为-1。 |
必需 | forceObj | vec3, list of 3 floats | 要施加的力/扭矩向量[x,y,z]。参见坐标系的标志。 |
必需 | posObj | vec3, list of 3 floats | 施加力的链接位置。仅适用于 applyExternalForce. 参见坐标系的标志。 |
必需 | flags | int | 指定力/位置的坐标系:笛卡尔世界坐标为 WORLD_FRAME,本地链接坐标为 LINK_FRAME。 |
可选 | physicsClientId | int | 如果您连接到多个服务器,您可以选一个。 |
2.1.8 程序结束
p.disconnect()#有连接就有断开
2.2 代码总体
import pybullet as p
import time
import pybullet_data
physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
p.setGravity(0,0,-9.81)
cylinderStartPos = [0,0,0.3]
cylinderStartOrientation = p.getQuaternionFromEuler([0,0,0])
planeId = p.loadURDF("plane.urdf")
cylinderId = p.loadURDF("/home/xzs/PyBullet_Practice/bullet3-master/data/cylinder1.urdf",cylinderStartPos,cylinderStartOrientation)
p.changeDynamics(planeId,-1,lateralFriction = 0,spinningFriction = 0,rollingFriction = 0)
for k in range(3):
for i in range (480):
p.applyExternalForce(cylinderId,-1,[1,0,0],[0,0,0.3],p.WORLD_FRAME)
p.stepSimulation()
time.sleep(1./240.)
for j in range (480):
p.applyExternalForce(cylinderId,-1,[-1,0,0],[0,0,0.3],p.WORLD_FRAME)
p.stepSimulation()
time.sleep(1./240.)
p.disconnect()
3. 效果展示
4. 总结
像完成一个大项目先要从小项目开始慢慢学习,别看项目虽小,但是五脏俱全啊,这是为后续做铺垫。后续学习内容我一会慢慢逐一完成发布到博客里,希望大家共同进步,有问题想要讨论的尽管发在评论区。由于我是初学者,可能有好多问题我也不会,到时候大家一起讨论,共同进步!!!