Python pybullet.getNumJoints函数代码示例

def getMotorJointStates(robot):
	joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
	joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
	joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1]
	joint_positions = [state[0] for state in joint_states]
	joint_velocities = [state[1] for state in joint_states]
	joint_torques = [state[3] for state in joint_states]
	return joint_positions, joint_velocities, joint_torques

	def addToScene(self, bodies):
		if self.parts is not None:
			parts = self.parts
		else:
			parts = {}

		if self.jdict is not None:
			joints = self.jdict
		else:
			joints = {}

		if self.ordered_joints is not None:
			ordered_joints = self.ordered_joints
		else:
			ordered_joints = []

		dump = 0
		for i in range(len(bodies)):
			if p.getNumJoints(bodies[i]) == 0:
				part_name, robot_name = p.getBodyInfo(bodies[i], 0)
				robot_name = robot_name.decode("utf8")
				part_name = part_name.decode("utf8")
				parts[part_name] = BodyPart(part_name, bodies, i, -1)
			for j in range(p.getNumJoints(bodies[i])):
				p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0)
				_,joint_name,_,_,_,_,_,_,_,_,_,_,part_name = p.getJointInfo(bodies[i], j)

				joint_name = joint_name.decode("utf8")
				part_name = part_name.decode("utf8")

				if dump: print("ROBOT PART '%s'" % part_name)
				if dump: print("ROBOT JOINT '%s'" % joint_name) # limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((joint_name,) + j.limits()) )

				parts[part_name] = BodyPart(part_name, bodies, i, j)

				if part_name == self.robot_name:
					self.robot_body = parts[part_name]

				if i == 0 and j == 0 and self.robot_body is None:  # if nothing else works, we take this as robot_body
					parts[self.robot_name] = BodyPart(self.robot_name, bodies, 0, -1)
					self.robot_body = parts[self.robot_name]

				if joint_name[:6] == "ignore":
					Joint(joint_name, bodies, i, j).disable_motor()
					continue

				if joint_name[:8] != "jointfix":
					joints[joint_name] = Joint(joint_name, bodies, i, j)
					ordered_joints.append(joints[joint_name])

					joints[joint_name].power_coef = 100.0

		return parts, joints, ordered_joints, self.robot_body

 def reset(self):
   objects = p.loadSDF(os.path.join(self.urdfRootPath,"kuka_iiwa/kuka_with_gripper2.sdf"))
   self.kukaUid = objects[0]
   #for i in range (p.getNumJoints(self.kukaUid)):
   #  print(p.getJointInfo(self.kukaUid,i))
   p.resetBasePositionAndOrientation(self.kukaUid,[-0.100000,0.000000,0.070000],[0.000000,0.000000,0.000000,1.000000])
   self.jointPositions=[ 0.006418, 0.413184, -0.011401, -1.589317, 0.005379, 1.137684, -0.006539, 0.000048, -0.299912, 0.000000, -0.000043, 0.299960, 0.000000, -0.000200 ]
   self.numJoints = p.getNumJoints(self.kukaUid)
   for jointIndex in range (self.numJoints):
     p.resetJointState(self.kukaUid,jointIndex,self.jointPositions[jointIndex])
     p.setJointMotorControl2(self.kukaUid,jointIndex,p.POSITION_CONTROL,targetPosition=self.jointPositions[jointIndex],force=self.maxForce)
   
   self.trayUid = p.loadURDF(os.path.join(self.urdfRootPath,"tray/tray.urdf"), 0.640000,0.075000,-0.190000,0.000000,0.000000,1.000000,0.000000)
   self.endEffectorPos = [0.537,0.0,0.5]
   self.endEffectorAngle = 0
   
   
   self.motorNames = []
   self.motorIndices = []
   
   for i in range (self.numJoints):
     jointInfo = p.getJointInfo(self.kukaUid,i)
     qIndex = jointInfo[3]
     if qIndex > -1:
       #print("motorname")
       #print(jointInfo[1])
       self.motorNames.append(str(jointInfo[1]))
       self.motorIndices.append(i)

 def setJointPosition(self, robot, position, kp=1.0, kv=0.3):
     import pybullet as p
     num_joints = p.getNumJoints(robot)
     zero_vec = [0.0] * num_joints
     if len(position) == num_joints:
         p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL,
             targetPositions=position, targetVelocities=zero_vec,
             positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)

	def initializeFromBulletBody(self, bodyUid, physicsClientId):
		self.initialize()

		#always create a base link
		baseLink = UrdfLink()
		baseLinkIndex = -1
		self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
		baseLink.link_name = 	p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8") 		
		self.linkNameToIndex[baseLink.link_name]=len(self.urdfLinks)
		self.urdfLinks.append(baseLink)
		
	
		#print(visualShapes)
		#optionally create child links and joints
		for j in range(p.getNumJoints(bodyUid,physicsClientId=physicsClientId)):
			jointInfo = p.getJointInfo(bodyUid,j,physicsClientId=physicsClientId)
			urdfLink = UrdfLink()
			self.convertLinkFromMultiBody(bodyUid, j, urdfLink,physicsClientId)
			urdfLink.link_name = jointInfo[12].decode("utf-8")
			self.linkNameToIndex[urdfLink.link_name]=len(self.urdfLinks)
			self.urdfLinks.append(urdfLink)
	
			urdfJoint = UrdfJoint()
			urdfJoint.link = urdfLink
			urdfJoint.joint_name = jointInfo[1].decode("utf-8")
			urdfJoint.joint_type = jointInfo[2]
			urdfJoint.joint_axis_xyz = jointInfo[13]
			orgParentIndex = jointInfo[16]
			if (orgParentIndex<0):
				urdfJoint.parent_name = baseLink.link_name
			else:
				parentJointInfo = p.getJointInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
				urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
			urdfJoint.child_name = urdfLink.link_name

			#todo, compensate for inertia/link frame offset
			
			dynChild = p.getDynamicsInfo(bodyUid,j,physicsClientId=physicsClientId)
			childInertiaPos = dynChild[3]
			childInertiaOrn = dynChild[4]
			parentCom2JointPos=jointInfo[14] 
			parentCom2JointOrn=jointInfo[15]
			tmpPos,tmpOrn = p.multiplyTransforms(childInertiaPos,childInertiaOrn,parentCom2JointPos,parentCom2JointOrn)
			tmpPosInv,tmpOrnInv = p.invertTransform(tmpPos,tmpOrn)
			dynParent = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
			parentInertiaPos = dynParent[3]
			parentInertiaOrn = dynParent[4]
			
			pos,orn = p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, tmpPosInv, tmpOrnInv)
			pos,orn_unused=p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, parentCom2JointPos,[0,0,0,1])
			
			urdfJoint.joint_origin_xyz = pos
			urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
			
			self.urdfJoints.append(urdfJoint)

def setupWorld():
	p.resetSimulation()
	p.loadURDF("planeMesh.urdf")
	kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
	for i in range (p.getNumJoints(kukaId)):
		p.setJointMotorControl2(kukaId,i,p.POSITION_CONTROL,force=0)
	for i in range (numObjects):
		cube = p.loadURDF("cube_small.urdf",[0,i*0.02,(i+1)*0.2])
		#p.changeDynamics(cube,-1,mass=100)
	p.stepSimulation()
	p.setGravity(0,0,-10)

def setJointPosition(robot, position, kp=1.0, kv=0.3):
	num_joints = p.getNumJoints(robot)
	zero_vec = [0.0] * num_joints
	if len(position) == num_joints:
		p.setJointMotorControlArray(robot, range(num_joints), p.POSITION_CONTROL,
			targetPositions=position, targetVelocities=zero_vec,
			positionGains=[kp] * num_joints, velocityGains=[kv] * num_joints)
	else:
		print("Not setting torque. "
			  "Expected torque vector of "
			  "length {}, got {}".format(num_joints, len(torque)))

 def reset(self):
   self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
   self.kp = 1
   self.kd = 0.1
   self.maxForce = 100
   nJoints = p.getNumJoints(self.quadruped)
   self.jointNameToId = {}
   for i in range(nJoints):
     jointInfo = p.getJointInfo(self.quadruped, i)
     self.jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
   self.resetPose()
   for i in range(100):
     p.stepSimulation()

def Step(stepIndex):
	for objectId in range(objectNum):
		record = log[stepIndex*objectNum+objectId]
		Id = record[2]
		pos = [record[3],record[4],record[5]]
		orn = [record[6],record[7],record[8],record[9]]
		p.resetBasePositionAndOrientation(Id,pos,orn)
		numJoints = p.getNumJoints(Id)
		for i in range (numJoints):
			jointInfo = p.getJointInfo(Id,i)
			qIndex = jointInfo[3]
			if qIndex > -1:
				p.resetJointState(Id,i,record[qIndex-7+17])

	def episode_restart(self):
		
		Scene.episode_restart(self)   # contains cpp_world.clean_everything()
		if (self.stadiumLoaded==0):
			self.stadiumLoaded=1
			
			# stadium_pose = cpp_household.Pose()
			# if self.zero_at_running_strip_start_line:
			#	 stadium_pose.set_xyz(27, 21, 0)  # see RUN_STARTLINE, RUN_RAD constants
			filename = os.path.join(pybullet_data.getDataPath(),"stadium_no_collision.sdf")
			self.ground_plane_mjcf = p.loadSDF(filename)
			
			for i in self.ground_plane_mjcf:
				p.changeDynamics(i,-1,lateralFriction=0.8, restitution=0.5)
				for j in range(p.getNumJoints(i)):
					p.changeDynamics(i,j,lateralFriction=0)

  def testJacobian(self):
    import pybullet as p

    clid = p.connect(p.SHARED_MEMORY)
    if (clid < 0):
      p.connect(p.DIRECT)

    time_step = 0.001
    gravity_constant = -9.81

    urdfs = [
        "TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf",
        "kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf"
    ]
    for urdf in urdfs:
      p.resetSimulation()
      p.setTimeStep(time_step)
      p.setGravity(0.0, 0.0, gravity_constant)

      robotId = p.loadURDF(urdf, useFixedBase=True)
      p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1])
      numJoints = p.getNumJoints(robotId)
      endEffectorIndex = numJoints - 1

      # Set a joint target for the position control and step the sim.
      self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)])
      p.stepSimulation()

      # Get the joint and link state directly from Bullet.
      mpos, mvel, mtorq = self.getMotorJointStates(robotId)

      result = p.getLinkState(robotId,
                              endEffectorIndex,
                              computeLinkVelocity=1,
                              computeForwardKinematics=1)
      link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
      # Get the Jacobians for the CoM of the end-effector link.
      # Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
      # The localPosition is always defined in terms of the link frame coordinates.

      zero_vec = [0.0] * len(mpos)
      jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec,
                                         zero_vec)

      assert (allclose(dot(jac_t, mvel), link_vt))
      assert (allclose(dot(jac_r, mvel), link_vr))
    p.disconnect()

  def setupWorld(self):
    numObjects = 50

    maximalCoordinates = False

    p.resetSimulation()
    p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
    p.loadURDF("planeMesh.urdf", useMaximalCoordinates=maximalCoordinates)
    kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", [0, 0, 10],
                        useMaximalCoordinates=maximalCoordinates)
    for i in range(p.getNumJoints(kukaId)):
      p.setJointMotorControl2(kukaId, i, p.POSITION_CONTROL, force=0)
    for i in range(numObjects):
      cube = p.loadURDF("cube_small.urdf", [0, i * 0.02, (i + 1) * 0.2])
      #p.changeDynamics(cube,-1,mass=100)
    p.stepSimulation()
    p.setGravity(0, 0, -10)

  def reset(self):
    self.initial_z = None
   
    objs = p.loadMJCF(os.path.join(self.urdfRootPath,"mjcf/humanoid_symmetric_no_ground.xml"),flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
    self.human = objs[0]
    self.jdict = {}
    self.ordered_joints = []
    self.ordered_joint_indices = []

    for j in range( p.getNumJoints(self.human) ):
      info = p.getJointInfo(self.human, j)
      link_name = info[12].decode("ascii")
      if link_name=="left_foot": self.left_foot = j
      if link_name=="right_foot": self.right_foot = j
      self.ordered_joint_indices.append(j)
      if info[2] != p.JOINT_REVOLUTE: continue
      jname = info[1].decode("ascii")
      self.jdict[jname] = j
      lower, upper = (info[8], info[9])
      self.ordered_joints.append( (j, lower, upper) )
      p.setJointMotorControl2(self.human, j, controlMode=p.VELOCITY_CONTROL, force=0)

    self.motor_names  = ["abdomen_z", "abdomen_y", "abdomen_x"]
    self.motor_power  = [100, 100, 100]
    self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"]
    self.motor_power += [100, 100, 300, 200]
    self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"]
    self.motor_power += [100, 100, 300, 200]
    self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"]
    self.motor_power += [75, 75, 75]
    self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
    self.motor_power += [75, 75, 75]
    self.motors = [self.jdict[n] for n in self.motor_names]
    print("self.motors")
    print(self.motors)
    print("num motors")
    print(len(self.motors))

def getJointStates(robot):
	joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
	joint_positions = [state[0] for state in joint_states]
	joint_velocities = [state[1] for state in joint_states]
	joint_torques = [state[3] for state in joint_states]
	return joint_positions, joint_velocities, joint_torques

import pybullet as p

usePort = True

if (usePort):
	id = p.connect(p.GRPC,"localhost:12345")
else:
	id = p.connect(p.GRPC,"localhost") 
print("id=",id)

if (id<0):
	print("Cannot connect to GRPC server")
	exit(0)
	
print ("Connected to GRPC")
r2d2 = p.loadURDF("r2d2.urdf")
print("numJoints = ", p.getNumJoints(r2d2))