本文整理汇总了Python中pybullet.getQuaternionFromEuler函数的典型用法代码示例。如果您正苦于以下问题：Python getQuaternionFromEuler函数的具体用法？Python getQuaternionFromEuler怎么用？Python getQuaternionFromEuler使用的例子？那么恭喜您, 这里精选的函数代码示例或许可以为您提供帮助。
在下文中一共展示了getQuaternionFromEuler函数的20个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于我们的系统推荐出更棒的Python代码示例。
示例1: joinUrdf
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
开发者ID:CGTGPY3G1，项目名称:bullet3，代码行数:58，代码来源:urdfEditor.py
示例2: _randomly_place_objects
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
开发者ID:AndrewMeadows，项目名称:bullet3，代码行数:27，代码来源:kuka_diverse_object_gym_env.py
示例3: _reset
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)
开发者ID:Valentactive，项目名称:bullet3，代码行数:21，代码来源:kukaCamGymEnv.py
示例4:
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.):
开发者ID:bulletphysics，项目名称:bullet3，代码行数:31，代码来源:getClosestPoints.py
示例5: createMultiBody
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:
#.........这里部分代码省略.........
开发者ID:AndrewMeadows，项目名称:bullet3，代码行数:101，代码来源:urdfEditor.py
示例6:
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()
开发者ID:AndrewMeadows，项目名称:bullet3，代码行数:23，代码来源:hello_pybullet.py
示例7: range
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
开发者ID:AndrewMeadows，项目名称:bullet3，代码行数:31，代码来源:vrhand_vive_tracker.py
示例8: applyAction
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)
开发者ID:AndrewMeadows，项目名称:bullet3，代码行数:77，代码来源:kuka.py
示例9: createMultiBody
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]
#.........这里部分代码省略.........
开发者ID:CGTGPY3G1，项目名称:bullet3，代码行数:101，代码来源:urdfEditor.py
示例10: createMultiBody
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)
#.........这里部分代码省略.........
开发者ID:AndrewMeadows，项目名称:bullet3，代码行数:101，代码来源:urdfEditor.py
示例11:
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,
开发者ID:bulletphysics，项目名称:bullet3，代码行数:31，代码来源:quadruped.py
示例12: Uber
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')
开发者ID:Learning-and-Intelligent-Systems，项目名称:lis_pr2_pkg，代码行数:31，代码来源:ros_pybullet.py
示例13:
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
开发者ID:bulletphysics，项目名称:bullet3，代码行数:25，代码来源:rollPitchYaw.py
示例14:
#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)
开发者ID:AndrewMeadows，项目名称:bullet3，代码行数:30，代码来源:testBike.py
示例15: range
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)
开发者ID:AndrewMeadows，项目名称:bullet3，代码行数:31，代码来源:createSphereMultiBodies.py
示例16: range
kukaId = p.loadURDF("kuka_iiwa/model.urdf", [0, 0, 0])
p.resetBasePositionAndOrientation(kukaId, [0, 0, 0], [0, 0, 0, 1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
#Joint damping coefficents. Using large values for the joints that we don't want to move.
jd = [100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 0.5]
#jd=[0.5,0.5,0.5,0.5,0.5,0.5,0.5]
p.setGravity(0, 0, 0)
while 1:
p.stepSimulation()
for i in range(1):
pos = [0, 0, 1.26]
orn = p.getQuaternionFromEuler([0, 0, 3.14])
jointPoses = p.calculateInverseKinematics(kukaId,
kukaEndEffectorIndex,
pos,
orn,
jointDamping=jd)
for i in range(numJoints):
p.setJointMotorControl2(bodyIndex=kukaId,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=jointPoses[i],
targetVelocity=0,
force=500,
positionGain=0.03,
开发者ID:bulletphysics，项目名称:bullet3，代码行数:31，代码来源:ik_end_effector_orientation.py
示例17: range
husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659])
for i in range (p.getNumJoints(husky)):
print(p.getJointInfo(husky,i))
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659)
ob = kukaId
jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ]
for jointIndex in range (p.getNumJoints(ob)):
p.resetJointState(ob,jointIndex,jointPositions[jointIndex])
#put kuka on top of husky
cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1])
baseorn = p.getQuaternionFromEuler([3.1415,0,0.3])
baseorn = [0,0,0,1]
#[0, 0, 0.707, 0.707]
#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1])
kukaEndEffectorIndex = 6
numJoints = p.getNumJoints(kukaId)
if (numJoints!=7):
exit()
#lower limits for null space
ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05]
#upper limits for null space
ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05]
#joint ranges for null space
开发者ID:AndrewMeadows，项目名称:bullet3，代码行数:31，代码来源:inverse_kinematics_husky_kuka.py
示例18: range
i = 4
p.setJointMotorControl2(kuka_gripper,
i,
p.POSITION_CONTROL,
targetPosition=e[ANALOG] * 0.05,
force=10)
i = 6
p.setJointMotorControl2(kuka_gripper,
i,
p.POSITION_CONTROL,
targetPosition=e[ANALOG] * 0.05,
force=10)
if sq_len < THRESHOLD * THRESHOLD:
eef_pos = e[POSITION]
eef_orn = p.getQuaternionFromEuler([0, -math.pi, 0])
joint_pos = p.calculateInverseKinematics(kuka,
6,
eef_pos,
eef_orn,
lowerLimits=LOWER_LIMITS,
upperLimits=UPPER_LIMITS,
jointRanges=JOINT_RANGE,
restPoses=REST_POSE,
jointDamping=JOINT_DAMP)
for i in range(len(joint_pos)):
p.setJointMotorControl2(kuka,
i,
p.POSITION_CONTROL,
targetPosition=joint_pos[i],
开发者ID:bulletphysics，项目名称:bullet3，代码行数:31，代码来源:vr_kuka_control.py
示例19:
visualShapeId = p.createVisualShape(shapeType=p.GEOM_MESH,
fileName="marble_cube.obj",
rgbaColor=[1, 1, 1, 1],
specularColor=[1, 1, 1],
visualFramePosition=shift,
meshScale=meshScale)
#collisionShapeId = p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="textures/marble_cube.obj", collisionFramePosition=shift,meshScale=meshScale)
collisionShapeId = -1
uiCube = p.createMultiBody(baseMass=0,
baseInertialFramePosition=[0, 0, 0],
baseCollisionShapeIndex=collisionShapeId,
baseVisualShapeIndex=visualShapeId,
basePosition=[0, 1, 0],
useMaximalCoordinates=True)
textOrn = p.getQuaternionFromEuler([0, 0, -1.5707963])
numLines = 1
lines = [-1] * numLines
p.stepSimulation()
#disable rendering during loading makes it much faster
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
#objects = [p.loadURDF("plane.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
kitchenObj = p.loadSDF("kitchens/1.sdf")
#objects = [p.loadURDF("samurai.urdf", 0.000000,0.000000,0.000000,0.000000,0.000000,0.000000,1.000000)]
objects = [
p.loadURDF("pr2_gripper.urdf", 0.500000, 0.300006, 0.700000, -0.000000, -0.000000, -0.000031,
1.000000)
]
pr2_gripper = objects[0]
print("pr2_gripper=")
开发者ID:bulletphysics，项目名称:bullet3，代码行数:31，代码来源:vr_kitchen_setup_vrSyncPython.py
示例20: range
p.loadURDF("plane.urdf", useMaximalCoordinates=True)#useMaximalCoordinates)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"physx_create_dominoes.json")
jran = 50
iran = 100
num=64
radius=0.1
numDominoes=0
for i in range (int(num*50)):
num=(radius*2*math.pi)/0.08
radius += 0.05/float(num)
orn = p.getQuaternionFromEuler([0,0,0.5*math.pi+math.pi*2*i/float(num)])
pos = [radius*math.cos(2*math.pi*(i/float(num))),radius*math.sin(2*math.pi*(i/float(num))), 0.03]
sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates)
numDominoes+=1
pos=[pos[0],pos[1],pos[2]+0.3]
orn = p.getQuaternionFromEuler([0,0,-math.pi/4.])
sphere = p.loadURDF("domino/domino.urdf",pos, orn, useMaximalCoordinates=useMaximalCoordinates)
print("numDominoes=",numDominoes)
#for j in range (20):
# for i in range (100):
# if (i<99):
# sphere = p.loadURDF("domino/domino.urdf",[i*0.04,1+j*.25,0.03], useMaximalCoordinates=useMaximalCoordinates)
开发者ID:jiapei100，项目名称:bullet3，代码行数:30，代码来源:otherPhysicsEngine.py
注：本文中的pybullet.getQuaternionFromEuler函数示例由纯净天空整理自Github/MSDocs等源码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。 |
客服电话
APP下载
官方微信
请发表评论