tweaks in urdfEditor.py and combineUrdf.py, put KUKA on top of Husky as test.
This commit is contained in:
Erwin Coumans
@@ -2,6 +2,7 @@ import pybullet_utils.bullet_client as bc
|
||||
import pybullet_utils.urdfEditor as ed
|
||||
import pybullet
|
||||
import pybullet_data
|
||||
import time
|
||||
|
||||
p0 = bc.BulletClient(connection_mode=pybullet.DIRECT)
|
||||
p0.setAdditionalSearchPath(pybullet_data.getDataPath())
|
||||
@@ -10,18 +11,20 @@ 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")
|
||||
husky = p1.loadURDF("husky/husky.urdf", flags=p0.URDF_USE_IMPLICIT_CYLINDER)
|
||||
kuka = p0.loadURDF("kuka_iiwa/model_free_base.urdf", flags=p0.URDF_USE_IMPLICIT_CYLINDER)
|
||||
|
||||
ed0 = ed.UrdfEditor()
|
||||
ed0.initializeFromBulletBody(frame, p1._client)
|
||||
ed0.initializeFromBulletBody(husky, p1._client)
|
||||
ed1 = ed.UrdfEditor()
|
||||
ed1.initializeFromBulletBody(door, p0._client)
|
||||
ed1.initializeFromBulletBody(kuka, p0._client)
|
||||
#ed1.saveUrdf("combined.urdf")
|
||||
|
||||
|
||||
parentLinkIndex = 0
|
||||
childLinkIndex = len(ed0.urdfLinks)
|
||||
insertJointIndex = len(ed0.urdfJoints)
|
||||
|
||||
#combine all links, and add a joint
|
||||
for link in ed1.urdfLinks:
|
||||
@@ -35,7 +38,7 @@ for joint in ed1.urdfJoints:
|
||||
jointPivotXYZInParent = [0.1,0,0.1]
|
||||
jointPivotRPYInParent = [0,0,0]
|
||||
|
||||
jointPivotXYZInChild = [-0.4,0,-0.4]
|
||||
jointPivotXYZInChild = [0,0,0]
|
||||
jointPivotRPYInChild = [0,0,0]
|
||||
jointPivotQuatInChild = p0.getQuaternionFromEuler(jointPivotRPYInChild)
|
||||
invJointPivotXYZInChild, invJointPivotQuatInChild = p0.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild)
|
||||
@@ -76,7 +79,11 @@ joint.joint_origin_rpy = jointPivotRPYInParent
|
||||
joint.joint_axis_xyz = [0,0,1]
|
||||
|
||||
|
||||
ed0.urdfJoints.append(joint)
|
||||
#the following commented line would crash PyBullet, it messes up the joint indexing/ordering
|
||||
#ed0.urdfJoints.append(joint)
|
||||
|
||||
#so make sure to insert the joint in the right place, to links/joints match
|
||||
ed0.urdfJoints.insert(insertJointIndex,joint)
|
||||
|
||||
ed0.saveUrdf("combined.urdf")
|
||||
|
||||
@@ -85,3 +92,10 @@ print(p1._client)
|
||||
print("p0.getNumBodies()=",p0.getNumBodies())
|
||||
print("p1.getNumBodies()=",p1.getNumBodies())
|
||||
|
||||
pgui = bc.BulletClient(connection_mode=pybullet.GUI)
|
||||
|
||||
ed0.createMultiBody([0,0,0],pgui._client)
|
||||
pgui.setRealTimeSimulation(1)
|
||||
|
||||
while (pgui.isConnected()):
|
||||
time.sleep(1./240.)
|
||||
|
||||
@@ -1,9 +1,12 @@
|
||||
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)
|
||||
|
||||
|
||||
|
||||
#for j in range (p.getNumJoints(0)):
|
||||
# p.setJointMotorControl2(0,j,p.VELOCITY_CONTROL,targetVelocity=0.1)
|
||||
p.setRealTimeSimulation(1)
|
||||
while (1):
|
||||
while (p.isConnected()):
|
||||
import time
|
||||
time.sleep(1./240.)
|
||||
|
||||
@@ -1,29 +0,0 @@
|
||||
<?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>
|
||||
|
||||
@@ -1,69 +0,0 @@
|
||||
<?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