Python pybullet.POSITION_CONTROL屬性代碼示例

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import POSITION_CONTROL [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 POSITION_CONTROL [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 POSITION_CONTROL [as 別名]
def set_gripper_open_position(self, robot, position=0, left=True, set_instantly=False, indices=None):
        if self.robot_type == 'pr2':
            indices_new = [79, 80, 81, 82] if left else [57, 58, 59, 60]
            positions = [position]*len(indices_new)
        elif self.robot_type == 'baxter':
            indices_new = [49, 51] if left else [27, 29]
            positions = [position, -position]
        elif self.robot_type == 'sawyer':
            indices_new = [20, 22]
            positions = [position, -position]
        elif self.robot_type == 'jaco':
            indices_new = [9, 11, 13]
            positions = [position, position, position]
        if indices is None:
            indices = indices_new

        if set_instantly:
            for i, j in enumerate(indices):
                p.resetJointState(robot, jointIndex=j, targetValue=positions[i], targetVelocity=0, physicsClientId=self.id)
        p.setJointMotorControlArray(robot, jointIndices=indices, controlMode=p.POSITION_CONTROL, targetPositions=positions, positionGains=np.array([0.05]*len(indices)), forces=[500]*len(indices), physicsClientId=self.id) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import POSITION_CONTROL [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 POSITION_CONTROL [as 別名]
def pre_grasp(self):
        # move fingers to pre-grasp configuration

        if self._control_arm is 'l':
            idx_thumb = self._joint_name_to_ids['l_hand::l_tj2']
            idx_fingers = [self._joint_name_to_ids[jn] for jn in self.joint_groups['l_hand']]
        else:
            idx_thumb = self._joint_name_to_ids['r_hand::r_tj2']
            idx_fingers = [self._joint_name_to_ids[jn] for jn in self.joint_groups['r_hand']]

        pos = [0.0] * len(idx_fingers)
        for i, idx in enumerate(idx_fingers):
            if idx == idx_thumb:
                pos[i] = 1.57

        p.setJointMotorControlArray(self.robot_id, idx_fingers, p.POSITION_CONTROL,
                                    targetPositions=pos,
                                    positionGains=[0.1] * len(idx_fingers),
                                    velocityGains=[1.0] * len(idx_fingers),
                                    physicsClientId=self._physics_client_id) 

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

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

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import POSITION_CONTROL [as 別名]
def set_position(self, position):
		self._p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,pybullet.POSITION_CONTROL, targetPosition=position) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import POSITION_CONTROL [as 別名]
def disable_motor(self):
		self._p.setJointMotorControl2(self.bodies[self.bodyIndex],self.jointIndex,controlMode=pybullet.POSITION_CONTROL, targetPosition=0, targetVelocity=0, positionGain=0.1, velocityGain=0.1, force=0) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import POSITION_CONTROL [as 別名]
def runAnimation(self, motorPositions):
        """Receives a list of positions and applies it motors"""
        p.setJointMotorControlArray(self.body, self.motors, controlMode=p.POSITION_CONTROL,
                                    targetPositions=motorPositions) 

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