Python pybullet.setJointMotorControl2方法代碼示例

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
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) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
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, {} 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
def control_joint_pos(body, joint, pos, max_vel=None, max_force=None):
        if max_vel is None:
            p.setJointMotorControl2(
                    bodyIndex=body,
                    jointIndex=joint,
                    controlMode=p.POSITION_CONTROL,
                    targetPosition=pos,
                    force=max_force)
        else:
            p.setJointMotorControl2(
                    bodyIndex=body,
                    jointIndex=joint,
                    controlMode=p.POSITION_CONTROL,
                    targetPosition=pos,
                    targetVelocity=max_vel,
                    force=max_force) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
def gripper(self, cmd, mode=pb.POSITION_CONTROL):
        '''
        Gripper commands need to be mirrored to simulate behavior of the actual
        UR5.
        '''
        pb.setJointMotorControl2(self.handle, self.left_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(
            self.handle, self.left_inner_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(self.handle, self.left_finger, mode,  cmd)
        pb.setJointMotorControl2(self.handle, self.left_fingertip, mode,  cmd)

        pb.setJointMotorControl2(self.handle, self.right_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(
            self.handle, self.right_inner_knuckle, mode,  -cmd)
        pb.setJointMotorControl2(self.handle, self.right_finger, mode,  cmd)
        pb.setJointMotorControl2(self.handle, self.right_fingertip, mode,  cmd) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
def setMotorsAngleInRealTimestep(self, motorTargetAngles, motorTargetTime, delayTime):
        if(motorTargetTime == 0):
            self._joint_targetPos = np.array(motorTargetAngles)
            for i in range(self._joint_number):
                p.setJointMotorControl2(bodyIndex=self._robot, jointIndex=self._joint_id[i], controlMode=p.POSITION_CONTROL,
                                        targetPosition=self._joint_targetPos[i],
                                        positionGain=self._kp, velocityGain=self._kd, force=self._torque, maxVelocity=self._max_velocity, physicsClientId=self._physicsClientId)
            time.sleep(delayTime)
        else:
            self._joint_currentPos = self._joint_targetPos
            self._joint_targetPos = np.array(motorTargetAngles)
            for i in range(self._joint_number):
                dydt = (self._joint_targetPos-self._joint_currentPos)/motorTargetTime
            internalTime = 0.0
            reft = time.time()
            while internalTime < motorTargetTime:
                internalTime = time.time() - reft
                for i in range(self._joint_number):
                    p.setJointMotorControl2(bodyIndex=self._robot, jointIndex=self._joint_id[i], controlMode=p.POSITION_CONTROL,
                                            targetPosition=self._joint_currentPos[i] + dydt[i] * internalTime,
                                            positionGain=self._kp, velocityGain=self._kd, force=self._torque, maxVelocity=self._max_velocity, physicsClientId=self._physicsClientId) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
def apply_action_fingers(self, action, obj_id=None):
        # move finger joints in position control
        assert len(action) == 2, ('finger joints are 2! The number of actions you passed is ', len(action))

        idx_fingers = [self._joint_name_to_ids['panda_finger_joint1'], self._joint_name_to_ids['panda_finger_joint2']]

        # use object id to check contact force and eventually stop the finger motion
        if obj_id is not None:
            _, forces = self.check_contact_fingertips(obj_id)
            # print("contact forces {}".format(forces))

            if forces[0] >= 20.0:
                action[0] = p.getJointState(self.robot_id, idx_fingers[0], physicsClientId=self._physics_client_id)[0]

            if forces[1] >= 20.0:
                action[1] = p.getJointState(self.robot_id, idx_fingers[1], physicsClientId=self._physics_client_id)[0]

        for i, idx in enumerate(idx_fingers):
            p.setJointMotorControl2(self.robot_id,
                                    idx,
                                    p.POSITION_CONTROL,
                                    targetPosition=action[i],
                                    force=10,
                                    maxVelocity=1,
                                    physicsClientId=self._physics_client_id) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
def setupWorld():
	p.resetSimulation()
        p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
	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) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
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) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
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) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
def sync_ik_robot(self, joint_positions, simulate=False, sync_last=True):
        """
        Force the internal robot model to match the provided joint angles.

        Args:
            joint_positions (list): a list or flat numpy array of joint positions.
            simulate (bool): If True, actually use physics simulation, else 
                write to physics state directly.
            sync_last (bool): If False, don't sync the last joint angle. This
                is useful for directly controlling the roll at the end effector.
        """
        num_joints = len(joint_positions)
        if not sync_last:
            num_joints -= 1
        for i in range(num_joints):
            if simulate:
                p.setJointMotorControl2(
                    self.ik_robot,
                    self.actual[i],
                    p.POSITION_CONTROL,
                    targetVelocity=0,
                    targetPosition=joint_positions[i],
                    force=500,
                    positionGain=0.5,
                    velocityGain=1.,
                )
            else:
                # Note that we use self.actual[i], and not i
                p.resetJointState(self.ik_robot, self.actual[i], joint_positions[i]) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
def sync_ik_robot(self, joint_positions, simulate=False, sync_last=True):
        """
        Force the internal robot model to match the provided joint angles.

        Args:
            joint_positions (list): a list or flat numpy array of joint positions.
            simulate (bool): If True, actually use physics simulation, else 
                write to physics state directly.
            sync_last (bool): If False, don't sync the last joint angle. This
                is useful for directly controlling the roll at the end effector.
        """

        num_joints = len(joint_positions)
        if not sync_last:
            num_joints -= 1
        for i in range(num_joints):
            if simulate:
                p.setJointMotorControl2(
                    self.ik_robot,
                    i,
                    p.POSITION_CONTROL,
                    targetVelocity=0,
                    targetPosition=joint_positions[i],
                    force=500,
                    positionGain=0.5,
                    velocityGain=1.,
                )
            else:
                p.resetJointState(self.ik_robot, i, joint_positions[i], 0) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
def sync_ik_robot(self, joint_positions, simulate=False, sync_last=True):
        """
        Force the internal robot model to match the provided joint angles.

        Args:
            joint_positions (list): a list or flat numpy array of joint positions.
            simulate (bool): If True, actually use physics simulation, else
                write to physics state directly.
            sync_last (bool): If False, don't sync the last joint angle. This
                is useful for directly controlling the roll at the end effector.
        """

        num_joints = len(joint_positions)
        if not sync_last:
            num_joints -= 1
        for i in range(num_joints):
            if simulate:
                p.setJointMotorControl2(
                    self.ik_robot,
                    i,
                    p.POSITION_CONTROL,
                    targetVelocity=0,
                    targetPosition=joint_positions[i],
                    force=500,
                    positionGain=0.5,
                    velocityGain=1.,
                )
            else:
                p.resetJointState(self.ik_robot, i, joint_positions[i], 0) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
def setJoints(self, joints):
        """Set joint targets for motor control in simulation
        
        Arguments:
            joints {dict} -- dict of joint name -> angle (float, radian)
        
        Raises:
            Exception: if a joint is not found, exception is raised

        Returns:
            applied {dict} -- dict of joint states (position, velocity, reaction forces, applied torque)
        """
        applied = {}

        for name in joints.keys():
            if name in self.joints:
                if name.endswith('_speed'):
                    p.setJointMotorControl2(
                    self.robot, self.joints[name], p.VELOCITY_CONTROL, targetVelocity=joints[name])
                else:
                    if name in self.maxTorques:
                        maxTorque = self.maxTorques[name]
                        p.setJointMotorControl2(
                            self.robot, self.joints[name], p.POSITION_CONTROL, joints[name], force=maxTorque)
                    else:
                        p.setJointMotorControl2(
                            self.robot, self.joints[name], p.POSITION_CONTROL, joints[name])

                applied[name] = p.getJointState(self.robot, self.joints[name])
            else:
                raise Exception("Can't find joint %s" % name)

        return applied 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
def control_joint_vel(body, joint, vel, max_force=None):
        p.setJointMotorControl2(
                bodyIndex=body,
                jointIndex=joint,
                controlMode=p.VELOCITY_CONTROL,
                targetVelocity=vel,
                force=max_force) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import setJointMotorControl2 [as 別名]
def control_joint_torque(body, joint, torque):
        p.setJointMotorControl2(
                bodyIndex=body,
                jointIndex=joint,
                controlMode=p.TORQUE_CONTROL,
                force=torque)