urdfEditor: add euler -> quaternion conversions (otherwise orientations get silently ignored)

urdfEditor: move combine code into UrdfEditor.joinUrdf
urdfEditor: Fix geom_length

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
+jointPivotXYZInParent = [0,0,0]
-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,0,0]
 jointPivotRPYInChild = [0,0,0]
-jointPivotQuatInChild = p0.getQuaternionFromEuler(jointPivotRPYInChild)
-invJointPivotXYZInChild, invJointPivotQuatInChild = p0.invertTransform(jointPivotXYZInChild,jointPivotQuatInChild)
 
-
+newjoint = ed0.joinUrdf(ed1, parentLinkIndex , jointPivotXYZInParent, jointPivotRPYInParent, jointPivotXYZInChild, jointPivotRPYInChild,  p0._client, p1._client)
-
-#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)
 
 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()):

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]
@@ -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)):
@@ -477,12 +541,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]
 				
 				if (len(shapeTypes)):
 					print("fileNames=",fileNames)
@@ -508,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,