Python pybullet.getQuaternionFromEuler函数代码示例

	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 _randomly_place_objects(self, urdfList):
    """Randomly places the objects in the bin.

    Args:
      urdfList: The list of urdf files to place in the bin.

    Returns:
      The list of object unique ID's.
    """


    # Randomize positions of each object urdf.
    objectUids = []
    for urdf_name in urdfList:
      xpos = 0.4 +self._blockRandom*random.random()
      ypos = self._blockRandom*(random.random()-.5)
      angle = np.pi/2 + self._blockRandom * np.pi * random.random()
      orn = p.getQuaternionFromEuler([0, 0, angle])
      urdf_path = os.path.join(self._urdfRoot, urdf_name)
      uid = p.loadURDF(urdf_path, [xpos, ypos, .15],
        [orn[0], orn[1], orn[2], orn[3]])
      objectUids.append(uid)
      # Let each object fall to the tray individual, to prevent object
      # intersection.
      for _ in range(500):
        p.stepSimulation()
    return objectUids

 def _reset(self):
   self.terminated = 0
   p.resetSimulation()
   p.setPhysicsEngineParameter(numSolverIterations=150)
   p.setTimeStep(self._timeStep)
   p.loadURDF(os.path.join(self._urdfRoot,"plane.urdf"),[0,0,-1])
   
   p.loadURDF(os.path.join(self._urdfRoot,"table/table.urdf"), 0.5000000,0.00000,-.820000,0.000000,0.000000,0.0,1.0)
   
   xpos = 0.5 +0.2*random.random()
   ypos = 0 +0.25*random.random()
   ang = 3.1415925438*random.random()
   orn = p.getQuaternionFromEuler([0,0,ang])
   self.blockUid =p.loadURDF(os.path.join(self._urdfRoot,"block.urdf"), xpos,ypos,-0.1,orn[0],orn[1],orn[2],orn[3])
           
   p.setGravity(0,0,-10)
   self._kuka = kuka.Kuka(urdfRootPath=self._urdfRoot, timeStep=self._timeStep)
   self._envStepCounter = 0
   p.stepSimulation()
   self._observation = self.getExtendedObservation()
   return np.array(self._observation)

import pybullet as p
import time
p.connect(p.GUI)
useCollisionShapeQuery = True
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
geom = p.createCollisionShape(p.GEOM_SPHERE, radius=0.1)
geomBox = p.createCollisionShape(p.GEOM_BOX, halfExtents=[0.2, 0.2, 0.2])
baseOrientationB = p.getQuaternionFromEuler([0, 0.3, 0])  #[0,0.5,0.5,0]
basePositionB = [1.5, 0, 1]
obA = -1
obB = -1

obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1])
obB = p.createMultiBody(baseMass=0,
                        baseCollisionShapeIndex=geomBox,
                        basePosition=basePositionB,
                        baseOrientation=baseOrientationB)

lineWidth = 3
colorRGB = [1, 0, 0]
lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0],
                            lineToXYZ=[0, 0, 0],
                            lineColorRGB=colorRGB,
                            lineWidth=lineWidth,
                            lifeTime=0)
pitch = 0
yaw = 0

while (p.isConnected()):
  pitch += 0.01
  if (pitch >= 3.1415 * 2.):

	def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0):
		#assume link[0] is base
		if (len(self.urdfLinks)==0):
			return -1

		base = self.urdfLinks[0]

		#v.tmp_collision_shape_ids=[]
		baseMass = base.urdf_inertial.mass
		baseCollisionShapeIndex = -1
		baseShapeTypeArray=[]
		baseRadiusArray=[]
		baseHalfExtentsArray=[]
		lengthsArray=[]
		fileNameArray=[]
		meshScaleArray=[]
		basePositionsArray=[]
		baseOrientationsArray=[]

		for v in base.urdf_collision_shapes:
			shapeType = v.geom_type
			baseShapeTypeArray.append(shapeType)
			baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
			baseRadiusArray.append(v.geom_radius)
			lengthsArray.append(v.geom_length)
			fileNameArray.append(v.geom_meshfilename)
			meshScaleArray.append(v.geom_meshscale)
			basePositionsArray.append(v.origin_xyz)
			orn=p.getQuaternionFromEuler(v.origin_rpy)
			baseOrientationsArray.append(orn)

		if (len(baseShapeTypeArray)):
			baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
					radii=baseRadiusArray,
					halfExtents=baseHalfExtentsArray,
					lengths=lengthsArray,
					fileNames=fileNameArray,
					meshScales=meshScaleArray,
					collisionFramePositions=basePositionsArray,
					collisionFrameOrientations=baseOrientationsArray,
					physicsClientId=physicsClientId)


		urdfVisuals = base.urdf_visual_shapes
		baseVisualShapeIndex = p.createVisualShapeArray(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],
                                                           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],
                                                           physicsClientId=physicsClientId)
#                 urdfVisual = base.urdf_visual_shapes[0]
#                 baseVisualShapeIndex = p.createVisualShape(shapeType=urdfVisual.geom_type,
#                                                                    halfExtents=[ext * 0.5 for ext in urdfVisual.geom_extents],
#                                                                    radius=urdfVisual.geom_radius,
#                                                                    length=urdfVisual.geom_length[0],
#                                                                    fileName=urdfVisual.geom_meshfilename,
#                                                                    meshScale=urdfVisual.geom_meshscale,
#                                                                    rgbaColor=urdfVisual.material_rgba,
#                                                                    visualFramePosition=urdfVisual.origin_xyz,
#                                                                    visualFrameOrientation=urdfVisual.origin_rpy,
#                                                                    physicsClientId=physicsClientId)


		linkMasses=[]
		linkCollisionShapeIndices=[]
		linkVisualShapeIndices=[]
		linkPositions=[]
		linkOrientations=[]
		linkMeshScaleArray=[]
		linkInertialFramePositions=[]
		linkInertialFrameOrientations=[]
		linkParentIndices=[]
		linkJointTypes=[]
		linkJointAxis=[]

		for joint in self.urdfJoints:
			link = joint.link
			linkMass = link.urdf_inertial.mass
			linkCollisionShapeIndex=-1
			linkVisualShapeIndex=-1
			linkPosition=[0,0,0]
			linkOrientation=[0,0,0]
			linkInertialFramePosition=[0,0,0]
			linkInertialFrameOrientation=[0,0,0]
			linkParentIndex=self.linkNameToIndex[joint.parent_name]
			linkJointType=joint.joint_type
			linkJointAx = joint.joint_axis_xyz
			linkShapeTypeArray=[]
			linkRadiusArray=[]
			linkHalfExtentsArray=[]
			lengthsArray=[]
			fileNameArray=[]
			linkPositionsArray=[]
			linkOrientationsArray=[]

			for v in link.urdf_collision_shapes:
#.........这里部分代码省略.........

import pybullet as p
from time import sleep

physicsClient = p.connect(p.GUI)

p.setGravity(0,0,-10)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,1]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)

useRealTimeSimulation = 0

if (useRealTimeSimulation):
	p.setRealTimeSimulation(1)

while 1:
	if (useRealTimeSimulation):
		p.setGravity(0,0,-10)
		sleep(0.01) # Time in seconds.
	else:
		p.stepSimulation()

objects = [p.loadURDF("jenga/jenga.urdf", 1.300000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.200000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.100000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 1.000000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.900000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("jenga/jenga.urdf", 0.800000,-0.700000,0.750000,0.000000,0.707107,0.000000,0.707107)]
objects = [p.loadURDF("table/table.urdf", 1.000000,-0.200000,0.000000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("cube_small.urdf", 0.950000,-0.100000,0.700000,0.000000,0.000000,0.707107,0.707107)]
objects = [p.loadURDF("sphere_small.urdf", 0.850000,-0.400000,0.700000,0.000000,0.000000,0.707107,0.707107)]

		
#load the MuJoCo MJCF hand
objects = p.loadMJCF("MPL/mpl2.xml")

hand=objects[0]
ho = p.getQuaternionFromEuler([3.14,-3.14/2,0])
hand_cid = p.createConstraint(hand,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[-0.05,0,0.02],[0.500000,0.300006,0.700000],ho)
print ("hand_cid")
print (hand_cid)
for i in range (p.getNumJoints(hand)):
	p.setJointMotorControl2(hand,i,p.POSITION_CONTROL,0,0)


#clamp in range 400-600
#minV = 400
#maxV = 600
minV = 250
maxV = 450


POSITION=1

 def applyAction(self, motorCommands):
   
   #print ("self.numJoints")
   #print (self.numJoints)
   if (self.useInverseKinematics):
     
     dx = motorCommands[0]
     dy = motorCommands[1]
     dz = motorCommands[2]
     da = motorCommands[3]
     fingerAngle = motorCommands[4]
     
     state = p.getLinkState(self.kukaUid,self.kukaEndEffectorIndex)
     actualEndEffectorPos = state[0]
     #print("pos[2] (getLinkState(kukaEndEffectorIndex)")
     #print(actualEndEffectorPos[2])
     
   
     
     self.endEffectorPos[0] = self.endEffectorPos[0]+dx
     if (self.endEffectorPos[0]>0.65):
       self.endEffectorPos[0]=0.65
     if (self.endEffectorPos[0]<0.50):
       self.endEffectorPos[0]=0.50
     self.endEffectorPos[1] = self.endEffectorPos[1]+dy
     if (self.endEffectorPos[1]<-0.17):
       self.endEffectorPos[1]=-0.17
     if (self.endEffectorPos[1]>0.22):
       self.endEffectorPos[1]=0.22
     
     #print ("self.endEffectorPos[2]")
     #print (self.endEffectorPos[2])
     #print("actualEndEffectorPos[2]")
     #print(actualEndEffectorPos[2])
     #if (dz<0 or actualEndEffectorPos[2]<0.5):
     self.endEffectorPos[2] = self.endEffectorPos[2]+dz
   
    
     self.endEffectorAngle = self.endEffectorAngle + da
     pos = self.endEffectorPos
     orn = p.getQuaternionFromEuler([0,-math.pi,0]) # -math.pi,yaw])
     if (self.useNullSpace==1):
       if (self.useOrientation==1):
         jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,self.ll,self.ul,self.jr,self.rp)
       else:
         jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,lowerLimits=self.ll, upperLimits=self.ul, jointRanges=self.jr, restPoses=self.rp)
     else:
       if (self.useOrientation==1):
         jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos,orn,jointDamping=self.jd)
       else:
         jointPoses = p.calculateInverseKinematics(self.kukaUid,self.kukaEndEffectorIndex,pos)
   
     #print("jointPoses")
     #print(jointPoses)
     #print("self.kukaEndEffectorIndex")
     #print(self.kukaEndEffectorIndex)
     if (self.useSimulation):
       for i in range (self.kukaEndEffectorIndex+1):
         #print(i)
         p.setJointMotorControl2(bodyUniqueId=self.kukaUid,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=self.maxForce,maxVelocity=self.maxVelocity, positionGain=0.3,velocityGain=1)
     else:
       #reset the joint state (ignoring all dynamics, not recommended to use during simulation)
       for i in range (self.numJoints):
         p.resetJointState(self.kukaUid,i,jointPoses[i])
     #fingers
     p.setJointMotorControl2(self.kukaUid,7,p.POSITION_CONTROL,targetPosition=self.endEffectorAngle,force=self.maxForce)
     p.setJointMotorControl2(self.kukaUid,8,p.POSITION_CONTROL,targetPosition=-fingerAngle,force=self.fingerAForce)
     p.setJointMotorControl2(self.kukaUid,11,p.POSITION_CONTROL,targetPosition=fingerAngle,force=self.fingerBForce)
     
     p.setJointMotorControl2(self.kukaUid,10,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
     p.setJointMotorControl2(self.kukaUid,13,p.POSITION_CONTROL,targetPosition=0,force=self.fingerTipForce)
     
     
   else:
     for action in range (len(motorCommands)):
       motor = self.motorIndices[action]
       p.setJointMotorControl2(self.kukaUid,motor,p.POSITION_CONTROL,targetPosition=motorCommands[action],force=self.maxForce)

	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

		#for i in range (len(self.urdfLinks)):
		#	print("link", i, "=",self.urdfLinks[i].link_name)


		base = self.urdfLinks[0]

		#v.tmp_collision_shape_ids=[]
		baseMass = base.urdf_inertial.mass
		baseCollisionShapeIndex = -1
		baseShapeTypeArray=[]
		baseRadiusArray=[]
		baseHalfExtentsArray=[]
		lengthsArray=[]
		fileNameArray=[]
		meshScaleArray=[]
		basePositionsArray=[]
		baseOrientationsArray=[]

		for v in base.urdf_collision_shapes:
			shapeType = v.geom_type
			baseShapeTypeArray.append(shapeType)
			baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
			baseRadiusArray.append(v.geom_radius)
			lengthsArray.append(v.geom_length)
			fileNameArray.append(v.geom_meshfilename)
			meshScaleArray.append(v.geom_meshscale)
			basePositionsArray.append(v.origin_xyz)
			orn=p.getQuaternionFromEuler(v.origin_rpy)
			baseOrientationsArray.append(orn)

		if (len(baseShapeTypeArray)):
			#print("fileNameArray=",fileNameArray)
			baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
					radii=baseRadiusArray,
					halfExtents=baseHalfExtentsArray,
					lengths=lengthsArray,
					fileNames=fileNameArray,
					meshScales=meshScaleArray,
					collisionFramePositions=basePositionsArray,
					collisionFrameOrientations=baseOrientationsArray,
					physicsClientId=physicsClientId)


		urdfVisuals = base.urdf_visual_shapes
		
		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 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=[p.getQuaternionFromEuler(v.origin_rpy) for v in urdfVisuals]                                                       
		baseVisualShapeIndex = -1

		if (len(shapeTypes)):
			#print("len(shapeTypes)=",len(shapeTypes))
			#print("len(halfExtents)=",len(halfExtents))
			#print("len(radii)=",len(radii))
			#print("len(lengths)=",len(lengths))
			#print("len(fileNames)=",len(fileNames))
			#print("len(meshScales)=",len(meshScales))
			#print("len(rgbaColors)=",len(rgbaColors))
			#print("len(visualFramePositions)=",len(visualFramePositions))
			#print("len(visualFrameOrientations)=",len(visualFrameOrientations))
			
                                                           
			baseVisualShapeIndex = p.createVisualShapeArray(shapeTypes=shapeTypes,
						halfExtents=halfExtents,radii=radii,lengths=lengths,fileNames=fileNames,
						meshScales=meshScales,rgbaColors=rgbaColors,visualFramePositions=visualFramePositions,
						visualFrameOrientations=visualFrameOrientations,physicsClientId=physicsClientId)

		linkMasses=[]
		linkCollisionShapeIndices=[]
		linkVisualShapeIndices=[]
		linkPositions=[]
		linkOrientations=[]
		
		linkInertialFramePositions=[]
		linkInertialFrameOrientations=[]
		linkParentIndices=[]
		linkJointTypes=[]
		linkJointAxis=[]

		for joint in self.urdfJoints:
			link = joint.link
			linkMass = link.urdf_inertial.mass
			linkCollisionShapeIndex=-1
			linkVisualShapeIndex=-1
			linkPosition=[0,0,0]
			linkOrientation=[0,0,0]
			linkInertialFramePosition=[0,0,0]
			linkInertialFrameOrientation=[0,0,0]
			linkParentIndex=self.linkNameToIndex[joint.parent_name]
#.........这里部分代码省略.........

	def createMultiBody(self, basePosition=[0,0,0],physicsClientId=0):
		#assume link[0] is base
		if (len(self.urdfLinks)==0):
			return -1
			
		base = self.urdfLinks[0]
		
		#v.tmp_collision_shape_ids=[]
		baseMass = base.urdf_inertial.mass
		print("baseMass=",baseMass)
		baseCollisionShapeIndex = -1

		baseShapeTypeArray=[]
		baseRadiusArray=[]
		baseHalfExtentsArray=[]
		lengthsArray=[]
		fileNameArray=[]
		meshScaleArray=[]
		basePositionsArray=[]
		baseOrientationsArray=[]
		
		for v in base.urdf_collision_shapes:
			print("base v.origin_xyz=",v.origin_xyz)
			print("base v.origin_rpy=",v.origin_rpy)
			shapeType = v.geom_type
			baseShapeTypeArray.append(shapeType)
			baseHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
			baseRadiusArray.append(v.geom_radius)
			lengthsArray.append(v.geom_length)
			fileNameArray.append(v.geom_meshfilename)
			meshScaleArray.append(v.geom_meshscale)
			basePositionsArray.append(v.origin_xyz)
			print("v.origin_rpy=",v.origin_rpy)
			orn=p.getQuaternionFromEuler(v.origin_rpy)
			baseOrientationsArray.append(orn)
			
		print("baseHalfExtentsArray=",baseHalfExtentsArray)
		if (len(baseShapeTypeArray)):
			baseCollisionShapeIndex = p.createCollisionShapeArray(shapeTypes=baseShapeTypeArray,
					radii=baseRadiusArray,
					halfExtents=baseHalfExtentsArray,
					lengths=lengthsArray,
					fileNames=fileNameArray,
					meshScales=meshScaleArray,
					collisionFramePositions=basePositionsArray,
					collisionFrameOrientations=baseOrientationsArray,
					physicsClientId=physicsClientId)


		print("baseCollisionShapeIndex=",baseCollisionShapeIndex)
			
			
		linkMasses=[]
		linkCollisionShapeIndices=[]
		linkVisualShapeIndices=[]
		linkPositions=[]
		linkOrientations=[]
		linkMeshScaleArray=[]
		linkInertialFramePositions=[]
		linkInertialFrameOrientations=[]
		linkParentIndices=[]
		linkJointTypes=[]
		linkJointAxis=[]
		
		for joint in self.urdfJoints:
			link = joint.link
			linkMass = link.urdf_inertial.mass
			
			print("linkMass=",linkMass)

			linkCollisionShapeIndex=-1
			linkVisualShapeIndex=-1
			linkPosition=[0,0,0]
			linkOrientation=[0,0,0]
			linkInertialFramePosition=[0,0,0]
			linkInertialFrameOrientation=[0,0,0]
			linkParentIndex=self.linkNameToIndex[joint.parent_name]
			print("parentId=",linkParentIndex)
			
			linkJointType=joint.joint_type
			print("linkJointType=",linkJointType, self.jointTypeToName[linkJointType]	)
			
			linkJointAx = joint.joint_axis_xyz
			
			linkShapeTypeArray=[]
			linkRadiusArray=[]
			linkHalfExtentsArray=[]
			lengthsArray=[]
			fileNameArray=[]
			linkPositionsArray=[]
			linkOrientationsArray=[]
			
			for v in link.urdf_collision_shapes:
				print("link v.origin_xyz=",v.origin_xyz)
				print("link v.origin_rpy=",v.origin_rpy)
				shapeType = v.geom_type
				linkShapeTypeArray.append(shapeType)
				linkHalfExtentsArray.append([0.5*v.geom_extents[0],0.5*v.geom_extents[1],0.5*v.geom_extents[2]])
				linkRadiusArray.append(v.geom_radius)
				lengthsArray.append(v.geom_length)
#.........这里部分代码省略.........

speed = 10
amplitude = 0.8
jump_amp = 0.5
maxForce = 3.5
kneeFrictionForce = 0
kp = 1
kd = .5
maxKneeForce = 1000

physId = p.connect(p.SHARED_MEMORY)
if (physId < 0):
  p.connect(p.GUI)
#p.resetSimulation()

angle = 0  # pick in range 0..0.2 radians
orn = p.getQuaternionFromEuler([0, angle, 0])
p.loadURDF("plane.urdf", [0, 0, 0], orn)
p.setPhysicsEngineParameter(numSolverIterations=numSolverIterations)
p.startStateLogging(p.STATE_LOGGING_GENERIC_ROBOT,
                    "genericlogdata.bin",
                    maxLogDof=16,
                    logFlags=p.STATE_LOG_JOINT_TORQUES)
p.setTimeOut(4000000)

p.setGravity(0, 0, 0)
p.setTimeStep(fixedTimeStep)

orn = p.getQuaternionFromEuler([0, 0, 0.4])
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3],
                       orn,

TORSO_JOINT_NAME = 'torso_lift_joint'

#Setup node
pub = rospy.Publisher('pybullet_cmds', uc_msg, queue_size=0)
rospy.init_node("pybullet_test")
rospy.loginfo("testing pybullet configurations")

#Setup controller
uc = Uber()
uc.command_joint_pose('l', [0]*7, time=2, blocking=True)

#Setup sim
physicsClient = connect(use_gui=True) #or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) 

pr2_start_orientation = p.getQuaternionFromEuler([0,0,0])
pr2_start_pose = [-.80*k,0,0]
pr2 = p.loadURDF("/home/demo/nishad/pddlstream/examples/pybullet/utils/models/pr2_description/pr2.urdf", pr2_start_pose, pr2_start_orientation, 
                      useFixedBase=True, globalScaling = 1 )

#The goal here is to get the arm positions of the actual robot using the uber controller
#and simulate them in pybullet

print str(uc.get_head_pose()) + ','

#Left Manipulator
left_joints = [joint_from_name(pr2, name) for name in ARM_JOINT_NAMES['left']]
l_joint_poses = uc.get_joint_positions('l')
#Right Manipulator
right_joints = [joint_from_name(pr2, name) for name in ARM_JOINT_NAMES['right']]
r_joint_poses = uc.get_joint_positions('r')

import pybullet as p
import time

cid = p.connect(p.SHARED_MEMORY)
if (cid < 0):
  p.connect(p.GUI)
q = p.loadURDF("quadruped/quadruped.urdf", useFixedBase=True)
rollId = p.addUserDebugParameter("roll", -1.5, 1.5, 0)
pitchId = p.addUserDebugParameter("pitch", -1.5, 1.5, 0)
yawId = p.addUserDebugParameter("yaw", -1.5, 1.5, 0)
fwdxId = p.addUserDebugParameter("fwd_x", -1, 1, 0)
fwdyId = p.addUserDebugParameter("fwd_y", -1, 1, 0)
fwdzId = p.addUserDebugParameter("fwd_z", -1, 1, 0)

while True:
  roll = p.readUserDebugParameter(rollId)
  pitch = p.readUserDebugParameter(pitchId)
  yaw = p.readUserDebugParameter(yawId)
  x = p.readUserDebugParameter(fwdxId)
  y = p.readUserDebugParameter(fwdyId)
  z = p.readUserDebugParameter(fwdzId)

  orn = p.getQuaternionFromEuler([roll, pitch, yaw])
  p.resetBasePositionAndOrientation(q, [x, y, z], orn)
  #p.stepSimulation()#not really necessary for this demo, no physics used

#coord	=	p.loadURDF("coordinateframe.urdf",[-2,0,1],useFixedBase=True)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(0)
#coordPos	=	[0,0,0]
#coordOrnEuler = [0,0,0]

#coordPos= [0.7000000000000004, 0, 0.22000000000000006]
#coordOrnEuler= [0, -0.2617993877991496, 0]

coordPos= [0.07, 0, -0.6900000000000004]
coordOrnEuler= [0, 0, 0]

coordPos2= [0, 0, 0 ]
coordOrnEuler2= [0, 0, 0]

invPos,invOrn=p.invertTransform(coordPos,p.getQuaternionFromEuler(coordOrnEuler))
mPos,mOrn = p.multiplyTransforms(invPos,invOrn, coordPos2,p.getQuaternionFromEuler(coordOrnEuler2))
eul = p.getEulerFromQuaternion(mOrn)
print("rpy=\"",eul[0],eul[1],eul[2],"\" xyz=\"", mPos[0],mPos[1], mPos[2])


shift=0
gui = 1


frame=0
states=[]
states.append(p.saveState())
#observations=[]
#observations.append(obs)
				

import pybullet as p
import time

useMaximalCoordinates = 0

p.connect(p.GUI)
#p.loadSDF("stadium.sdf",useMaximalCoordinates=useMaximalCoordinates)
monastryId = concaveEnv =p.createCollisionShape(p.GEOM_MESH,fileName="samurai_monastry.obj",flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
orn = p.getQuaternionFromEuler([1.5707963,0,0])
p.createMultiBody (0,monastryId, baseOrientation=orn)

sphereRadius = 0.05
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])

mass = 1
visualShapeId = -1


for i in range (5):
	for j in range (5):
		for k in range (5):
			if (k&2):
				sphereUid = p.createMultiBody(mass,colSphereId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1],useMaximalCoordinates=useMaximalCoordinates)
			else:
				sphereUid = p.createMultiBody(mass,colBoxId,visualShapeId,[-i*2*sphereRadius,j*2*sphereRadius,k*2*sphereRadius+1], useMaximalCoordinates=useMaximalCoordinates)
			p.changeDynamics(sphereUid,-1,spinningFriction=0.001, rollingFriction=0.001,linearDamping=0.0)
			


p.setGravity(0,0,-10)