pybullet构建闭环结构笔记

yosob

已于 2022-11-26 23:17:00 修改

阅读量981

点赞数 4

分类专栏： pybullet 文章标签： python

于 2022-08-24 22:50:14 首次发布

本文链接：https://blog.csdn.net/yosob/article/details/126513114

版权

pybullet 专栏收录该内容

1 篇文章 0 订阅

订阅专栏

pybullet构建闭环结构笔记

- 一。 pybullet构建loop

一。 pybullet构建loop

1. reference

[双足机器人]pybullet单足仿真-解决机器人中存在闭链机构的问题 - 知乎 (zhihu.com)

2. createconstraint

pybullet中不支持loop structure的直接输入，但是支持使用 createConstraint 函数在程序中构建
在这里插入图片描述

createconstraint函数如下：显然这只是一个python到c++的接口

def createConstraint(*args, **kwargs): # real signature unknown
    """ Create a constraint between two bodies. Returns a (int) unique id, if successfull. """
    pass

input：
在这里插入图片描述

输入参数很多，但是其实主要的就是index和position两个方面。

3. index关系

首先解决一些link index的问题。pybullet中并没有一个专门查link index的函数，但是在getjointinfo中顺带查到parent link index。经过实验，joint是和child link直接联系在一起的。就是说，parent link index 是0，child link index 是1，那么joint index 也是1，joint index等于child link index
在这里插入图片描述

这段话传达了差不多的意思，所以index的问题解决了，base link index就是-1，别的index和对应joint index一样。

4. pybullet中的坐标关系

现在来解决坐标关系。

pybullet中的base，link，joints关系如下图所示

在这里插入图片描述

pybullet将joint和child link视作一个整体，child frame相对于parent frame的position是joint link frame到parent center of mass coordinate。

通过 getLinkState 函数能够获得mass坐标系和urdf link frame的关系，而根据DH参数的设计urdf link frame一般就是joint frame，这个就很方便了。

5. example

我通过四个100 * 30 * 10的杆子搞了个四杆机构，两孔间距70

在这里插入图片描述

其构建的URDF坐标系如图：分别位于base的下底面和base对面杆的上底面

在这里插入图片描述

三个joint通过URDF确定，那个c的就是使用程序确定的

代码如下：

p.resetJointState(theloop.id, 0, 0)
p.resetJointState(theloop.id, 1, 0)
p.resetJointState(theloop.id, 2, 0)

p.createConstraint(parentBodyUniqueId=theloop.id,
                   parentLinkIndex=2,
                   childBodyUniqueId=theloop.id,
                   childLinkIndex=-1,
                   jointType=p.JOINT_POINT2POINT,
                   jointAxis=[0, 0, 0],
                   parentFramePosition=[0.035, 0, 0.005],
                   childFramePosition=[-0.035, 0, -0.005])
p.setJointMotorControl2(theloop.id, 0, p.POSITION_CONTROL, targetPosition=0, force=0.5)
p.setJointMotorControl2(theloop.id, 1, p.VELOCITY_CONTROL, targetVelocity=0, force=0.5)
p.setJointMotorControl2(theloop.id, 2, p.VELOCITY_CONTROL, targetVelocity=0, force=0.5)

现在我们来看一个更加复杂的例子：

这是一个欠驱动的手指模型，我们想要构建这个带有loop结构的模型就势必需要构建约束，于是想要通过绿色的两个p2p的约束将红色的导轨与手指的基座相连。
请添加图片描述为了弄清楚其中的坐标关系，先查看手指的URDF文件。此处的URDF文件是使用ROS的swtourdf插件生成的。
由这一段的origin关系结合上文的link坐标系关系，可以得到link的mass frame只是joint frame的平移，其rpy都是0 。仔细观察URDF文件就可以发现，其中的link origin rpy总是为0，而joint origin rpy值各异，由此可得urdf中坐标系的转向全部由joint frame来承担，mass frame只是joint frame的平移。

有了上述的结论，计算约束点和mass frame的相对位置关系就简单起来。

我们在solidworks中建立约束的目标点，点14

请添加图片描述另外一个点同上计算。

所以程序应该这样构建：

# build loop1
loop1_1 = p.createConstraint(parentBodyUniqueId=self.hand_id,
                             parentLinkIndex=-1 + thumb_num + bias,
                             childBodyUniqueId=self.hand_id,
                             childLinkIndex=5 + thumb_num + bias,
                             jointType=p.JOINT_POINT2POINT,
                             jointAxis=[0, 0, 0],
                             parentFramePosition=[0.007, -0.0051, -0.024],
                             childFramePosition=[0, 0, -0.0208],
                             physicsClientId=self.client_id)
p.changeConstraint(userConstraintUniqueId=loop1_1, maxForce=maxforce,
                   physicsClientId=self.client_id)
self.loops_constraint_idlist.append(loop1_1)
loop1_2 = p.createConstraint(parentBodyUniqueId=self.hand_id,
                             parentLinkIndex=-1 + thumb_num + bias,
                             childBodyUniqueId=self.hand_id,
                             childLinkIndex=5 + thumb_num + bias,
                             jointType=p.JOINT_POINT2POINT,
                             jointAxis=[0, 0, 0],
                             parentFramePosition=[0.030, -0.0051, 0.0153],
                             childFramePosition=[0, 0, 0.025],
                             physicsClientId=self.client_id)
p.changeConstraint(userConstraintUniqueId=loop1_2, maxForce=maxforce,
                   physicsClientId=self.client_id)
self.loops_constraint_idlist.append(loop1_2)