diff options
Diffstat (limited to 'examples')
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> + |