add example to joint two URDF files, using the urdfEditor (combineUrdf.py)

4 files changed, 194 insertions, 0 deletions

diff --git a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py
new file mode 100644
index 000000000..e80c58073
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py
@@ -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())
+
diff --git a/examples/pybullet/gym/pybullet_utils/examples/combined.py b/examples/pybullet/gym/pybullet_utils/examples/combined.py
new file mode 100644
index 000000000..349cc887b
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_utils/examples/combined.py
@@ -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.)
diff --git a/examples/pybullet/gym/pybullet_utils/examples/door1.urdf b/examples/pybullet/gym/pybullet_utils/examples/door1.urdf
new file mode 100644
index 000000000..4323d70ce
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_utils/examples/door1.urdf
@@ -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>
+
diff --git a/examples/pybullet/gym/pybullet_utils/examples/frame1.urdf b/examples/pybullet/gym/pybullet_utils/examples/frame1.urdf
new file mode 100644
index 000000000..1f8d2f3f2
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_utils/examples/frame1.urdf
@@ -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>
+