diff --git a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py index a7192ce70..09066451a 100644 --- a/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py +++ b/examples/pybullet/gym/pybullet_utils/examples/combineUrdf.py @@ -13,7 +13,7 @@ p1.setAdditionalSearchPath(pybullet_data.getDataPath()) #can also connect using different modes, GUI, SHARED_MEMORY, TCP, UDP, SHARED_MEMORY_SERVER, GUI_SERVER 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) +kuka = p0.loadURDF("kuka_iiwa/model.urdf") ed0 = ed.UrdfEditor() ed0.initializeFromBulletBody(husky, p1._client) @@ -23,67 +23,15 @@ ed1.initializeFromBulletBody(kuka, p0._client) parentLinkIndex = 0 -childLinkIndex = len(ed0.urdfLinks) -insertJointIndex = len(ed0.urdfJoints) -#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] +jointPivotXYZInParent = [0,0,0] jointPivotRPYInParent = [0,0,0] + jointPivotXYZInChild = [0,0,0] 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] - - -#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) +newjoint = ed0.joinUrdf(ed1, parentLinkIndex , jointPivotXYZInParent, jointPivotRPYInParent, jointPivotXYZInChild, jointPivotRPYInChild, p0._client, p1._client) ed0.saveUrdf("combined.urdf") @@ -94,7 +42,8 @@ print("p1.getNumBodies()=",p1.getNumBodies()) pgui = bc.BulletClient(connection_mode=pybullet.GUI) -ed0.createMultiBody([0,0,0],pgui._client) +orn=[0,0,0,1] +ed0.createMultiBody([0,0,0],orn, pgui._client) pgui.setRealTimeSimulation(1) while (pgui.isConnected()): diff --git a/examples/pybullet/gym/pybullet_utils/urdfEditor.py b/examples/pybullet/gym/pybullet_utils/urdfEditor.py index 9ec226cfb..61416e591 100644 --- a/examples/pybullet/gym/pybullet_utils/urdfEditor.py +++ b/examples/pybullet/gym/pybullet_utils/urdfEditor.py @@ -29,7 +29,7 @@ class UrdfVisual(object): self.geom_type = p.GEOM_BOX self.geom_radius = 1 self.geom_extents = [7,8,9] - self.geom_length=[10] + self.geom_length=10 self.geom_meshfilename = "meshfile" self.geom_meshscale=[1,1,1] self.material_rgba = [1,0,0,1] @@ -117,8 +117,8 @@ class UrdfEditor(object): if (v[2]==p.GEOM_SPHERE): urdfCollision.geom_radius = v[3][0] if (v[2]==p.GEOM_MESH): - urdfCollision.geom_meshfilename = v[4].decode("utf-8") - urdfCollision.geom_meshscale = v[3] + urdfCollision.geom_meshfilename = v[4].decode("utf-8") + urdfCollision.geom_meshscale = v[3] if (v[2]==p.GEOM_CYLINDER): urdfCollision.geom_length=v[3][0] urdfCollision.geom_radius=v[3][1] @@ -255,7 +255,7 @@ class UrdfEditor(object): prec=precision) file.write(str) if urdfCollision.geom_type == p.GEOM_MESH: - str = '\t\t\t\t scale=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"\n'.format(urdfCollision.geom_meshfilename,\ + str = '\t\t\t\t\n'.format(urdfCollision.geom_meshfilename,\ urdfCollision.geom_meshscale[0],urdfCollision.geom_meshscale[1],urdfCollision.geom_meshscale[2],prec=precision) file.write(str) if urdfCollision.geom_type == p.GEOM_CYLINDER: @@ -339,7 +339,71 @@ class UrdfEditor(object): #def addLink(...) - def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0): + + + + def joinUrdf(self, childEditor, parentLinkIndex=0, jointPivotXYZInParent=[0,0,0], jointPivotRPYInParent=[0,0,0], jointPivotXYZInChild=[0,0,0], jointPivotRPYInChild=[0,0,0], parentPhysicsClientId=0, childPhysicsClientId=0): + + childLinkIndex = len(self.urdfLinks) + insertJointIndex = len(self.urdfJoints) + + #combine all links, and add a joint + + for link in childEditor.urdfLinks: + self.linkNameToIndex[link.link_name]=len(self.urdfLinks) + self.urdfLinks.append(link) + for joint in childEditor.urdfJoints: + self.urdfJoints.append(joint) + #add a new joint between a particular + + + + + jointPivotQuatInChild = p.getQuaternionFromEuler(jointPivotRPYInChild) + invJointPivotXYZInChild, invJointPivotQuatInChild = p.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild) + + #apply this invJointPivot***InChild to all inertial, visual and collision element in the child link + #inertial + pos, orn = p.multiplyTransforms(self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz,p.getQuaternionFromEuler(self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild, physicsClientId=parentPhysicsClientId) + self.urdfLinks[childLinkIndex].urdf_inertial.origin_xyz = pos + self.urdfLinks[childLinkIndex].urdf_inertial.origin_rpy = p.getEulerFromQuaternion(orn) + #all visual + for v in self.urdfLinks[childLinkIndex].urdf_visual_shapes: + pos, orn = p.multiplyTransforms(v.origin_xyz,p.getQuaternionFromEuler(v.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild) + v.origin_xyz = pos + v.origin_rpy = p.getEulerFromQuaternion(orn) + #all collision + for c in self.urdfLinks[childLinkIndex].urdf_collision_shapes: + pos, orn = p.multiplyTransforms(c.origin_xyz,p.getQuaternionFromEuler(c.origin_rpy),invJointPivotXYZInChild,invJointPivotQuatInChild) + c.origin_xyz = pos + c.origin_rpy = p.getEulerFromQuaternion(orn) + + + childLink = self.urdfLinks[childLinkIndex] + parentLink = self.urdfLinks[parentLinkIndex] + + joint = UrdfJoint() + joint.link = childLink + joint.joint_name = "joint_dummy1" + joint.joint_type = p.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] + + #the following commented line would crash PyBullet, it messes up the joint indexing/ordering + #self.urdfJoints.append(joint) + + #so make sure to insert the joint in the right place, to links/joints match + self.urdfJoints.insert(insertJointIndex,joint) + return joint + + + + def createMultiBody(self, basePosition=[0,0,0],baseOrientation=[0,0,0,1], physicsClientId=0): #assume link[0] is base if (len(self.urdfLinks)==0): return -1 @@ -392,12 +456,12 @@ class UrdfEditor(object): shapeTypes=[v.geom_type for v in urdfVisuals] halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals] radii=[v.geom_radius for v in urdfVisuals] - lengths=[v.geom_length[0] for v in urdfVisuals] + lengths=[v.geom_length for v in urdfVisuals] fileNames=[v.geom_meshfilename for v in urdfVisuals] meshScales=[v.geom_meshscale for v in urdfVisuals] rgbaColors=[v.material_rgba for v in urdfVisuals] visualFramePositions=[v.origin_xyz for v in urdfVisuals] - visualFrameOrientations=[v.origin_rpy for v in urdfVisuals] + visualFrameOrientations=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals] baseVisualShapeIndex = -1 if (len(shapeTypes)): @@ -422,7 +486,7 @@ class UrdfEditor(object): linkVisualShapeIndices=[] linkPositions=[] linkOrientations=[] - linkMeshScaleArray=[] + linkInertialFramePositions=[] linkInertialFrameOrientations=[] linkParentIndices=[] @@ -446,6 +510,7 @@ class UrdfEditor(object): linkHalfExtentsArray=[] lengthsArray=[] fileNameArray=[] + linkMeshScaleArray=[] linkPositionsArray=[] linkOrientationsArray=[] @@ -470,19 +535,19 @@ class UrdfEditor(object): collisionFramePositions=linkPositionsArray, collisionFrameOrientations=linkOrientationsArray, physicsClientId=physicsClientId) - + urdfVisuals = link.urdf_visual_shapes linkVisualShapeIndex = -1 shapeTypes=[v.geom_type for v in urdfVisuals] halfExtents=[[ext * 0.5 for ext in v.geom_extents] for v in urdfVisuals] radii=[v.geom_radius for v in urdfVisuals] - lengths=[v.geom_length[0] for v in urdfVisuals] + lengths=[v.geom_length for v in urdfVisuals] fileNames=[v.geom_meshfilename for v in urdfVisuals] meshScales=[v.geom_meshscale for v in urdfVisuals] rgbaColors=[v.material_rgba for v in urdfVisuals] visualFramePositions=[v.origin_xyz for v in urdfVisuals] - visualFrameOrientations=[v.origin_rpy for v in urdfVisuals] - + visualFrameOrientations=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals] + if (len(shapeTypes)): print("fileNames=",fileNames) @@ -507,8 +572,9 @@ class UrdfEditor(object): baseCollisionShapeIndex=baseCollisionShapeIndex, baseVisualShapeIndex=baseVisualShapeIndex, basePosition=basePosition, + baseOrientation=baseOrientation, baseInertialFramePosition=base.urdf_inertial.origin_xyz, - baseInertialFrameOrientation=base.urdf_inertial.origin_rpy, + baseInertialFrameOrientation=p.getQuaternionFromEuler(base.urdf_inertial.origin_rpy), linkMasses=linkMasses, linkCollisionShapeIndices=linkCollisionShapeIndices, linkVisualShapeIndices=linkVisualShapeIndices, diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 55c79c0f4..405d849ce 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -605,6 +605,10 @@ static PyObject* pybullet_syncBodyInfo(PyObject* self, PyObject* args, PyObject* b3PhysicsClientHandle sm = 0; int physicsClientId = 0; static char* kwlist[] = {"physicsClientId", NULL}; + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId)) { return NULL; @@ -616,9 +620,6 @@ static PyObject* pybullet_syncBodyInfo(PyObject* self, PyObject* args, PyObject* return NULL; } - b3SharedMemoryCommandHandle command; - b3SharedMemoryStatusHandle statusHandle; - int statusType; command = b3InitSyncBodyInfoCommand(sm); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);