Python pybullet.setJointMotorControl2函数代码示例

 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 setMotorAngleById(self, motorId, desiredAngle):
   p.setJointMotorControl2(bodyIndex=self.quadruped,
                           jointIndex=motorId,
                           controlMode=p.POSITION_CONTROL,
                           targetPosition=desiredAngle,
                           positionGain=self.kp,
                           velocityGain=self.kd,
                           force=self.maxForce)

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 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 _step(self, action):
    p.stepSimulation()
#    time.sleep(self.timeStep)
    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
    theta, theta_dot, x, x_dot = self.state
    force = action
    p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(action + self.state[3]))

    done =  x < -self.x_threshold \
                or x > self.x_threshold \
                or theta < -self.theta_threshold_radians \
                or theta > self.theta_threshold_radians
    reward = 1.0

    return np.array(self.state), reward, done, {}

  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 _step(self, action):
    force = self.force_mag if action==1 else -self.force_mag

    p.setJointMotorControl2(self.cartpole, 0, p.TORQUE_CONTROL, force=force)
    p.stepSimulation()

    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
    theta, theta_dot, x, x_dot = self.state

    done =  x < -self.x_threshold \
                or x > self.x_threshold \
                or theta < -self.theta_threshold_radians \
                or theta > self.theta_threshold_radians
    done = bool(done)
    reward = 1.0
    #print("state=",self.state)
    return np.array(self.state), reward, done, {}

  def _step(self, action):
    p.stepSimulation()
#    time.sleep(self.timeStep)
    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
    theta, theta_dot, x, x_dot = self.state
    
    dv = 0.1
    deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]
    
    p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3]))

    done =  x < -self.x_threshold \
                or x > self.x_threshold \
                or theta < -self.theta_threshold_radians \
                or theta > self.theta_threshold_radians
    reward = 1.0

    return np.array(self.state), reward, done, {}

  def _reset(self):
#    print("-----------reset simulation---------------")
    p.resetSimulation()
    self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
    self.timeStep = 0.01
    p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
    p.setGravity(0,0, -10)
    p.setTimeStep(self.timeStep)
    p.setRealTimeSimulation(0)

    initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
    initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
    p.resetJointState(self.cartpole, 1, initialAngle)
    p.resetJointState(self.cartpole, 0, initialCartPos)

    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]

    return np.array(self.state)

  def _reset(self):
#    print("-----------reset simulation---------------")
    p.resetSimulation()
    self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
    p.changeDynamics(self.cartpole, -1, linearDamping=0, angularDamping=0)
    p.changeDynamics(self.cartpole, 0, linearDamping=0, angularDamping=0)
    p.changeDynamics(self.cartpole, 1, linearDamping=0, angularDamping=0)
    self.timeStep = 0.02
    p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
    p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, force=0)
    p.setGravity(0,0, -9.8)
    p.setTimeStep(self.timeStep)
    p.setRealTimeSimulation(0)

    randstate = self.np_random.uniform(low=-0.05, high=0.05, size=(4,))
    p.resetJointState(self.cartpole, 1, randstate[0], randstate[1])
    p.resetJointState(self.cartpole, 0, randstate[2], randstate[3])
    #print("randstate=",randstate)
    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
    #print("self.state=", self.state)
    return np.array(self.state)

  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))

import pybullet as p
import time

p.connect(p.GUI)
cartpole=p.loadURDF("cartpole.urdf")
p.setRealTimeSimulation(1)
p.setJointMotorControl2(cartpole,1,p.POSITION_CONTROL,targetPosition=1000,targetVelocity=0,force=1000, positionGain=1, velocityGain=0, maxVelocity=0.5)
while (1):
	p.setGravity(0,0,-10)
	js = p.getJointState(cartpole,1)
	print("position=",js[0],"velocity=",js[1])
	time.sleep(0.01)

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
ORIENTATION=2
BUTTONS=6

p.setRealTimeSimulation(1)

jointNamesToIndex={}


p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
vision = p.loadURDF("vision60.urdf",[0,0,0.4],useFixedBase=False)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)


for j in range(p.getNumJoints(vision)):
	jointInfo = p.getJointInfo(vision,j)
	jointInfoName = jointInfo[1].decode("utf-8") 
	print("joint ",j," = ",jointInfoName, "type=",jointTypeNames[jointInfo[2]])
	jointNamesToIndex[jointInfoName ]=j
	#print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName])
	p.setJointMotorControl2(vision,j,p.VELOCITY_CONTROL,targetVelocity=0, force=jointFriction)


chassis_right_center = jointNamesToIndex['chassis_right_center']
motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_joint']
motor_front_rightS_joint = jointNamesToIndex['motor_front_rightS_joint']
hip_front_rightR_joint = jointNamesToIndex['hip_front_rightR_joint']
knee_front_rightR_joint = jointNamesToIndex['knee_front_rightR_joint']
motor_front_rightL_joint = jointNamesToIndex['motor_front_rightL_joint']
motor_back_rightR_joint = jointNamesToIndex['motor_back_rightR_joint']
motor_back_rightS_joint = jointNamesToIndex['motor_back_rightS_joint']
hip_back_rightR_joint = jointNamesToIndex['hip_back_rightR_joint']
knee_back_rightR_joint = jointNamesToIndex['knee_back_rightR_joint']
motor_back_rightL_joint = jointNamesToIndex['motor_back_rightL_joint']
chassis_left_center = jointNamesToIndex['chassis_left_center']
motor_front_leftL_joint = jointNamesToIndex['motor_front_leftL_joint']

    p.loadURDF("plane.urdf", 0.000000, 0.000000, -.300000, 0.000000, 0.000000, 0.000000, 1.000000)
]
objects = [
    p.loadURDF("quadruped/minitaur.urdf", [-0.000046, -0.000068, 0.200774],
               [-0.000701, 0.000387, -0.000252, 1.000000],
               useFixedBase=False)
]
ob = objects[0]
jointPositions = [
    0.000000, 1.531256, 0.000000, -2.240112, 1.527979, 0.000000, -2.240646, 1.533105, 0.000000,
    -2.238254, 1.530335, 0.000000, -2.238298, 0.000000, -1.528038, 0.000000, 2.242656, -1.525193,
    0.000000, 2.244008, -1.530011, 0.000000, 2.240683, -1.528687, 0.000000, 2.240517
]
for ji in range(p.getNumJoints(ob)):
  p.resetJointState(ob, ji, jointPositions[ji])
  p.setJointMotorControl2(bodyIndex=ob, jointIndex=ji, controlMode=p.VELOCITY_CONTROL, force=0)

cid0 = p.createConstraint(1, 3, 1, 6, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
                          [0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
                          [0.000000, 0.000000, 0.000000, 1.000000],
                          [0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid0, maxForce=500.000000)
cid1 = p.createConstraint(1, 16, 1, 19, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
                          [0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
                          [0.000000, 0.000000, 0.000000, 1.000000],
                          [0.000000, 0.000000, 0.000000, 1.000000])
p.changeConstraint(cid1, maxForce=500.000000)
cid2 = p.createConstraint(1, 9, 1, 12, p.JOINT_POINT2POINT, [0.000000, 0.000000, 0.000000],
                          [0.000000, 0.005000, 0.200000], [0.000000, 0.010000, 0.200000],
                          [0.000000, 0.000000, 0.000000, 1.000000],
                          [0.000000, 0.000000, 0.000000, 1.000000])