add example to joint two URDF files, using the urdfEditor (combineUrdf.py)
This commit is contained in:
Erwin Coumans
87
examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py
Normal file
87
examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py
Normal file
@@ -0,0 +1,87 @@
|
||||
import pybullet_utils.bullet_client as bc
|
||||
import pybullet_utils.urdfEditor as ed
|
||||
import pybullet
|
||||
import pybullet_data
|
||||
|
||||
p0 = bc.BulletClient(connection_mode=pybullet.DIRECT)
|
||||
p0.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
|
||||
p1 = bc.BulletClient(connection_mode=pybullet.DIRECT)
|
||||
p1.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
|
||||
#can also connect using different modes, GUI, SHARED_MEMORY, TCP, UDP, SHARED_MEMORY_SERVER, GUI_SERVER
|
||||
#pgui = bc.BulletClient(connection_mode=pybullet.GUI)
|
||||
|
||||
frame = p1.loadURDF("frame1.urdf")
|
||||
door = p0.loadURDF("door1.urdf")
|
||||
|
||||
ed0 = ed.UrdfEditor()
|
||||
ed0.initializeFromBulletBody(frame, p1._client)
|
||||
ed1 = ed.UrdfEditor()
|
||||
ed1.initializeFromBulletBody(door, p0._client)
|
||||
|
||||
parentLinkIndex = 0
|
||||
childLinkIndex = len(ed0.urdfLinks)
|
||||
|
||||
#combine all links, and add a joint
|
||||
for link in ed1.urdfLinks:
|
||||
ed0.linkNameToIndex[link.link_name]=len(ed0.urdfLinks)
|
||||
ed0.urdfLinks.append(link)
|
||||
for joint in ed1.urdfJoints:
|
||||
ed0.urdfJoints.append(joint)
|
||||
#add a new joint between a particular
|
||||
|
||||
|
||||
jointPivotXYZInParent = [0.1,0,0.1]
|
||||
jointPivotRPYInParent = [0,0,0]
|
||||
|
||||
jointPivotXYZInChild = [-0.4,0,-0.4]
|
||||
jointPivotRPYInChild = [0,0,0]
|
||||
jointPivotQuatInChild = p0.getQuaternionFromEuler(jointPivotRPYInChild)
|
||||
invJointPivotXYZInChild, invJointPivotQuatInChild = p0.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild)
|
||||
|
||||
|
||||
|
||||
#apply this invJointPivot***InChild to all inertial, visual and collision element in the child link
|
||||
#inertial
|
||||
pos, orn = p0.multiplyTransforms(ed0.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz,p0.getQuaternionFromEuler(ed0.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild)
|
||||
ed0.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz = pos
|
||||
ed0.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy = p0.getEulerFromQuaternion(orn)
|
||||
#all visual
|
||||
for v in ed0.urdfLinks[childLinkIndex].urdf_visual_shapes:
|
||||
pos, orn = p0.multiplyTransforms(v.origin_xyz,p0.getQuaternionFromEuler(v.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild)
|
||||
v.origin_xyz = pos
|
||||
v.origin_rpy = p0.getEulerFromQuaternion(orn)
|
||||
#all collision
|
||||
for c in ed0.urdfLinks[childLinkIndex].urdf_collision_shapes:
|
||||
pos, orn = p0.multiplyTransforms(c.origin_xyz,p0.getQuaternionFromEuler(c.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild)
|
||||
c.origin_xyz = pos
|
||||
c.origin_rpy = p0.getEulerFromQuaternion(orn)
|
||||
|
||||
|
||||
childLink = ed0.urdfLinks[childLinkIndex]
|
||||
parentLink = ed0.urdfLinks[parentLinkIndex]
|
||||
|
||||
|
||||
joint = ed.UrdfJoint()
|
||||
joint.link = childLink
|
||||
joint.joint_name = "joint_dummy1"
|
||||
joint.joint_type = p0.JOINT_REVOLUTE
|
||||
joint.joint_lower_limit = 0
|
||||
joint.joint_upper_limit = -1
|
||||
joint.parent_name = parentLink.link_name
|
||||
joint.child_name = childLink.link_name
|
||||
joint.joint_origin_xyz = jointPivotXYZInParent
|
||||
joint.joint_origin_rpy = jointPivotRPYInParent
|
||||
joint.joint_axis_xyz = [0,0,1]
|
||||
|
||||
|
||||
ed0.urdfJoints.append(joint)
|
||||
|
||||
ed0.saveUrdf("combined.urdf")
|
||||
|
||||
print(p0._client)
|
||||
print(p1._client)
|
||||
print("p0.getNumBodies()=",p0.getNumBodies())
|
||||
print("p1.getNumBodies()=",p1.getNumBodies())
|
||||
|
||||
@@ -0,0 +1,9 @@
|
||||
import pybullet as p
|
||||
p.connect(p.GUI)
|
||||
p.loadURDF("combined.urdf", useFixedBase=True)
|
||||
for j in range (p.getNumJoints(0)):
|
||||
p.setJointMotorControl2(0,j,p.VELOCITY_CONTROL,targetVelocity=0.1)
|
||||
p.setRealTimeSimulation(1)
|
||||
while (1):
|
||||
import time
|
||||
time.sleep(1./240.)
|
||||
29
examples/pybullet/gym/pybullet_utils/examples/door1.urdf
Normal file
29
examples/pybullet/gym/pybullet_utils/examples/door1.urdf
Normal file
@@ -0,0 +1,29 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="urdf_door">
|
||||
<link name="door">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<mass value="1.0"/>
|
||||
<inertia ixx="0.048966669" ixy="0" ixz="0" iyy="0.046466667" iyz="0" izz="0.0041666669"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.8 0.05 0.8"/>
|
||||
</geometry>
|
||||
<material name="doormat0">
|
||||
<color rgba="0.8 0.8 0.3 1" />
|
||||
</material>
|
||||
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<box size="0.8 0.05 0.8"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
</robot>
|
||||
|
||||
69
examples/pybullet/gym/pybullet_utils/examples/frame1.urdf
Normal file
69
examples/pybullet/gym/pybullet_utils/examples/frame1.urdf
Normal file
@@ -0,0 +1,69 @@
|
||||
<?xml version="0.0" ?>
|
||||
<robot name="urdf_door">
|
||||
|
||||
<link name="frame">
|
||||
<inertial>
|
||||
<origin rpy="0 0 0" xyz="0 0 0.35"/>
|
||||
<mass value="1.0"/>
|
||||
<inertia ixx="0.1" ixy="0" ixz="0" iyy="0.1" iyz="0" izz="0.1"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0.05 0 0.5"/>
|
||||
<geometry>
|
||||
<box size="0.1 0.1 1"/>
|
||||
</geometry>
|
||||
<material name="framemat0">
|
||||
<color
|
||||
rgba="0.9 0.4 0. 1" />
|
||||
</material>
|
||||
</visual>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0.95 0 0.5"/>
|
||||
<geometry>
|
||||
<box size="0.1 0.1 1"/>
|
||||
</geometry>
|
||||
<material name="framemat0"/>
|
||||
</visual>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0.5 0 0.95"/>
|
||||
<geometry>
|
||||
<box size="1 0.1 0.1"/>
|
||||
</geometry>
|
||||
<material name="framemat0"/>
|
||||
</visual>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0.5 0 0.05"/>
|
||||
<geometry>
|
||||
<box size="1 0.1 0.1"/>
|
||||
</geometry>
|
||||
<material name="framemat0"/>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.05 0 0.5"/>
|
||||
<geometry>
|
||||
<box size="0.1 0.1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.95 0 0.5"/>
|
||||
<geometry>
|
||||
<box size="0.1 0.1 1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.5 0 0.95"/>
|
||||
<geometry>
|
||||
<box size="1 0.1 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin rpy="0 0 0" xyz="0.5 0 0.05"/>
|
||||
<geometry>
|
||||
<box size="1 0.1 0.1"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
|
||||
</link>
|
||||
|
||||
</robot>
|
||||
|
||||
Reference in New Issue
Block a user