Python pybullet.getNumJoints方法代碼示例

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getNumJoints [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 getNumJoints [as 別名]
def enforce_joint_limits(self, body):
        # Enforce joint limits
        joint_states = p.getJointStates(body, jointIndices=list(range(p.getNumJoints(body, physicsClientId=self.id))), physicsClientId=self.id)
        joint_positions = np.array([x[0] for x in joint_states])
        lower_limits = []
        upper_limits = []
        for j in range(p.getNumJoints(body, physicsClientId=self.id)):
            joint_info = p.getJointInfo(body, j, physicsClientId=self.id)
            joint_name = joint_info[1]
            joint_pos = joint_positions[j]
            lower_limit = joint_info[8]
            upper_limit = joint_info[9]
            if lower_limit == 0 and upper_limit == -1:
                lower_limit = -1e10
                upper_limit = 1e10
            lower_limits.append(lower_limit)
            upper_limits.append(upper_limit)
            # print(joint_name, joint_pos, lower_limit, upper_limit)
            if joint_pos < lower_limit:
                p.resetJointState(body, jointIndex=j, targetValue=lower_limit, targetVelocity=0, physicsClientId=self.id)
            elif joint_pos > upper_limit:
                p.resetJointState(body, jointIndex=j, targetValue=upper_limit, targetVelocity=0, physicsClientId=self.id)
        lower_limits = np.array(lower_limits)
        upper_limits = np.array(upper_limits)
        return lower_limits, upper_limits 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getNumJoints [as 別名]
def findGraspFrame(self):
        '''
        Helper function to look up the grasp frame associated with a robot.
        '''
        joints = pb.getNumJoints(self.handle)
        grasp_idx = None
        for i in xrange(joints):
            idx, name, jtype, qidx, \
                uidx, flags, damping, friction, \
                lower, upper, max_force, max_vel, \
                link_name = pb.getJointInfo(self.handle, i)
            #info = pb.getJointInfo(self.handle, i)
            #print(info)
            #link_name = info[12]
            assert(isinstance(link_name, str))
            if link_name == self.grasp_link:
                grasp_idx = i
                break
        return grasp_idx 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getNumJoints [as 別名]
def setRobot(self, robot, physicsClientId=None):
        self._robot = robot
        if physicsClientId != None:
            self._physicsClientId = physicsClientId
        jointNameToId = {}
        joint_id_list = []
        joint_pos_list = []
        self._joint_number = 0
        for i in range(p.getNumJoints(self._robot, physicsClientId=self._physicsClientId)):
            jointInfo = p.getJointInfo(self._robot, i, physicsClientId=self._physicsClientId)
            if jointInfo[2] == 0:
                joint_id_list.append(jointInfo[0])
                joint_pos_list.append(p.getJointState(self._robot, jointInfo[0], physicsClientId=self._physicsClientId)[0])
                jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
                self._joint_number += 1
        self._joint_id = np.array(joint_id_list, dtype=np.int32)
        self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
        self._joint_currentPos = np.array(joint_pos_list, dtype=np.float) 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getNumJoints [as 別名]
def setRobot(self, robot, physicsClientId=None):
        self._robot = robot
        if physicsClientId != None:
            self._physicsClientId = physicsClientId

        joint_id_list = []
        joint_pos_list = []
        self._joint_number = 0
        for i in range(p.getNumJoints(robot)):
            jointInfo = p.getJointInfo(robot, i)
            if jointInfo[2] == 0:
                joint_id_list.append(jointInfo[0])
                joint_pos_list.append(p.getJointState(robot, jointInfo[0])[0])
                self._joint_number += 1
        print(self._joint_number)
        self._joint_id = np.array(joint_id_list, dtype=np.int32)
        self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
        self._joint_currentPos = np.array(joint_pos_list, dtype=np.float) 

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

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

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getNumJoints [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 getNumJoints [as 別名]
def getMotorJointStates(self, robot):
        import pybullet as p
        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 

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

        # initialize robot
        self.body = p.loadURDF("../soccer_description/models/soccerbot/model.xacro")
        self.state = RobotState()
        self.imu = -1
        self.imuMeasurements = JointMeasurement()
        self.joints = []
        self.motors = []

        # create a list of joints and find the IMU
        for i in range(p.getNumJoints(self.body)):
            self.joints.append(i)
            jointInfo = p.getJointInfo(self.body, i)
            if jointInfo[1].decode('ascii') == "torso_imu":
                self.imu = jointInfo[0]

        if self.imu == -1:
            raise AttributeError("Could not find robot's imu sensor from joint list")

        # rearrange joint order to match the order of positions found in the csv files. See:
        # https://docs.google.com/spreadsheets/d/1KgIYwm3fNen8yjLEa-FEWq-GnRUnBjyg4z64nZ2uBv8/edit#gid=775054312
        self.motors = self.joints[2:14] + self.joints[17:21] + self.joints[14:16] + self.joints[0:2]

        # initialize animations
        self.getupBackAnimation = Animation("./trajectories/getupback.csv")
        self.getupFrontAnimation = Animation("./trajectories/getupfront.csv")
        self.readyAnimation = Animation("./trajectories/ready.csv")
        self.standingAnimation = Animation("./trajectories/standing.csv")
        self.activeAnimation = None 

# 需要導入模塊: import pybullet [as 別名]
# 或者: from pybullet import getNumJoints [as 別名]
def setup_inverse_kinematics(self, urdf_path):
        """
        This function is responsible for doing any setup for inverse kinematics.
        Inverse Kinematics maps end effector (EEF) poses to joint angles that
        are necessary to achieve those poses. 
        """

        # These indices come from the urdf file we're using
        self.effector_right = 27
        self.effector_left = 45

        # Use PyBullet to handle inverse kinematics.
        # Set up a connection to the PyBullet simulator.
        p.connect(p.DIRECT)
        p.resetSimulation()

        self.ik_robot = p.loadURDF(urdf_path, (0, 0, 0), useFixedBase=1)

        # Relevant joints we care about. Many of the joints are fixed and don't count, so
        # we need this second map to use the right ones.
        self.actual = [13, 14, 15, 16, 17, 19, 20, 31, 32, 33, 34, 35, 37, 38]

        self.num_joints = p.getNumJoints(self.ik_robot)
        n = p.getNumJoints(self.ik_robot)
        self.rest = []
        self.lower = []
        self.upper = []
        self.ranges = []
        for i in range(n):
            info = p.getJointInfo(self.ik_robot, i)
            # Retrieve lower and upper ranges for each relevant joint
            if info[3] > -1:
                self.rest.append(p.getJointState(self.ik_robot, i)[0])
                self.lower.append(info[8])
                self.upper.append(info[9])
                self.ranges.append(info[9] - info[8])

        # Simulation will update as fast as it can in real time, instead of waiting for
        # step commands like in the non-realtime case.
        p.setRealTimeSimulation(1)