Python pybullet.addUserDebugLine方法代码示例

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def debug_gui(self):
        ws = self._workspace_lim
        p1 = [ws[0][0], ws[1][0], ws[2][0]]  # xmin,ymin
        p2 = [ws[0][1], ws[1][0], ws[2][0]]  # xmax,ymin
        p3 = [ws[0][1], ws[1][1], ws[2][0]]  # xmax,ymax
        p4 = [ws[0][0], ws[1][1], ws[2][0]]  # xmin,ymax

        p.addUserDebugLine(p1, p2, lineColorRGB=[0, 0, 1], lineWidth=2.0, lifeTime=0, physicsClientId=self._physics_client_id)
        p.addUserDebugLine(p2, p3, lineColorRGB=[0, 0, 1], lineWidth=2.0, lifeTime=0, physicsClientId=self._physics_client_id)
        p.addUserDebugLine(p3, p4, lineColorRGB=[0, 0, 1], lineWidth=2.0, lifeTime=0, physicsClientId=self._physics_client_id)
        p.addUserDebugLine(p4, p1, lineColorRGB=[0, 0, 1], lineWidth=2.0, lifeTime=0, physicsClientId=self._physics_client_id)

        p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0], parentObjectUniqueId=self.robot_id,
                           parentLinkIndex=-1, physicsClientId=self._physics_client_id)
        p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0], parentObjectUniqueId=self.robot_id,
                           parentLinkIndex=-1, physicsClientId=self._physics_client_id)
        p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1], parentObjectUniqueId=self.robot_id,
                           parentLinkIndex=-1, physicsClientId=self._physics_client_id)

        p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0], parentObjectUniqueId=self.robot_id,
                           parentLinkIndex=self.end_eff_idx, physicsClientId=self._physics_client_id)
        p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0], parentObjectUniqueId=self.robot_id,
                           parentLinkIndex=self.end_eff_idx, physicsClientId=self._physics_client_id)
        p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1], parentObjectUniqueId=self.robot_id,
                           parentLinkIndex=self.end_eff_idx, physicsClientId=self._physics_client_id) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def drawInertiaBox(parentUid, parentLinkIndex, color):
	dyn = p.getDynamicsInfo(parentUid, parentLinkIndex)
	mass=dyn[0]
	frictionCoeff=dyn[1]
	inertia = dyn[2]
	if (mass>0):
		Ixx = inertia[0]
		Iyy = inertia[1]
		Izz = inertia[2]
		boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass);
		boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass);
		boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass);
  
		halfExtents = [boxScaleX,boxScaleY,boxScaleZ]
		pts = [[halfExtents[0],halfExtents[1],halfExtents[2]],
				 [-halfExtents[0],halfExtents[1],halfExtents[2]],
				 [halfExtents[0],-halfExtents[1],halfExtents[2]],
				 [-halfExtents[0],-halfExtents[1],halfExtents[2]],
				 [halfExtents[0],halfExtents[1],-halfExtents[2]],
				 [-halfExtents[0],halfExtents[1],-halfExtents[2]],
				 [halfExtents[0],-halfExtents[1],-halfExtents[2]],
				 [-halfExtents[0],-halfExtents[1],-halfExtents[2]]]
	
		
		p.addUserDebugLine(pts[0],pts[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
		p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
		p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
		p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	
		p.addUserDebugLine(pts[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
		p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
		p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
		p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	
		p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
		p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
		p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
		p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def drawInertiaBox(parentUid, parentLinkIndex):
	mass,frictionCoeff, inertia =p.getDynamicsInfo(bodyUniqueId=parentUid,linkIndex=parentLinkIndex, flags = p.DYNAMICS_INFO_REPORT_INERTIA)
	Ixx = inertia[0]
	Iyy = inertia[1]
	Izz = inertia[2]
	boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass);
	boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass);
	boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass);
  
	halfExtents = [boxScaleX,boxScaleY,boxScaleZ]
	pts = [[halfExtents[0],halfExtents[1],halfExtents[2]],
				 [-halfExtents[0],halfExtents[1],halfExtents[2]],
				 [halfExtents[0],-halfExtents[1],halfExtents[2]],
				 [-halfExtents[0],-halfExtents[1],halfExtents[2]],
				 [halfExtents[0],halfExtents[1],-halfExtents[2]],
				 [-halfExtents[0],halfExtents[1],-halfExtents[2]],
				 [halfExtents[0],-halfExtents[1],-halfExtents[2]],
				 [-halfExtents[0],-halfExtents[1],-halfExtents[2]]]
	
	color=[1,0,0]
	p.addUserDebugLine(pts[0],pts[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	
	p.addUserDebugLine(pts[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	
	p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
	p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def getExtendedObservation(self):
     self._observation = self._kuka.getObservation()
     gripperState  = p.getLinkState(self._kuka.kukaUid,self._kuka.kukaGripperIndex)
     gripperPos = gripperState[0]
     gripperOrn = gripperState[1]
     blockPos,blockOrn = p.getBasePositionAndOrientation(self.blockUid)

     invGripperPos,invGripperOrn = p.invertTransform(gripperPos,gripperOrn)
     gripperMat = p.getMatrixFromQuaternion(gripperOrn)
     dir0 = [gripperMat[0],gripperMat[3],gripperMat[6]]
     dir1 = [gripperMat[1],gripperMat[4],gripperMat[7]]
     dir2 = [gripperMat[2],gripperMat[5],gripperMat[8]]

     gripperEul =  p.getEulerFromQuaternion(gripperOrn)
     #print("gripperEul")
     #print(gripperEul)
     blockPosInGripper,blockOrnInGripper = p.multiplyTransforms(invGripperPos,invGripperOrn,blockPos,blockOrn)
     projectedBlockPos2D =[blockPosInGripper[0],blockPosInGripper[1]]
     blockEulerInGripper = p.getEulerFromQuaternion(blockOrnInGripper)
     #print("projectedBlockPos2D")
     #print(projectedBlockPos2D)
     #print("blockEulerInGripper")
     #print(blockEulerInGripper)

     #we return the relative x,y position and euler angle of block in gripper space
     blockInGripperPosXYEulZ =[blockPosInGripper[0],blockPosInGripper[1],blockEulerInGripper[2]]

     #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir0[0],gripperPos[1]+dir0[1],gripperPos[2]+dir0[2]],[1,0,0],lifeTime=1)
     #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir1[0],gripperPos[1]+dir1[1],gripperPos[2]+dir1[2]],[0,1,0],lifeTime=1)
     #p.addUserDebugLine(gripperPos,[gripperPos[0]+dir2[0],gripperPos[1]+dir2[1],gripperPos[2]+dir2[2]],[0,0,1],lifeTime=1)

     self._observation.extend(list(blockInGripperPosXYEulZ))
     return self._observation 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def drawDebugLines(self):
        """Updates the drawing of debug lines"""
        self.currentLine = 0
        if time.time() - self.lastLinesDraw > 0.05:
            for line in self.lines:
                if 'from' in line:
                    if line['update'] == True:
                        p.addUserDebugLine(line['from'], line['to'], line['color'], 2, line['duration'])
                        line['update'] = False
                    else:
                        del line['from']
                line['from'] = line['to']

            self.lastLinesDraw = time.time() 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def draw_camera_pos(self):
        pb.removeAllUserDebugItems()
        start = self.camera_pos
        end_x = start + np.dot(self.camera_rot, np.array([0.1, 0, 0, 1.0]))[0:3]
        pb.addUserDebugLine(start, end_x, [1, 0, 0], 5)
        end_y = start + np.dot(self.camera_rot, np.array([0, 0.1, 0, 1.0]))[0:3]
        pb.addUserDebugLine(start, end_y, [0, 1, 0], 5)
        end_z = start + np.dot(self.camera_rot, np.array([0, 0, 0.1, 1.0]))[0:3]
        pb.addUserDebugLine(start, end_z, [0, 0, 1], 5) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def _createDebugLine(self):
        """
        INTERNAL METHOD, create all debug lines needed for simulating the
        lasers
        """
        for i in range(NUM_RAY * NUM_LASER):
            self.ray_ids.append(pybullet.addUserDebugLine(
                self.ray_from[i],
                self.ray_to[i],
                RAY_MISS_COLOR,
                parentObjectUniqueId=self.robot_model,
                parentLinkIndex=self.laser_id,
                physicsClientId=self.physics_client)) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def debug_gui(self):

        ws = self._workspace_lim
        p1 = [ws[0][0], ws[1][0], ws[2][0]]  # xmin,ymin
        p2 = [ws[0][1], ws[1][0], ws[2][0]]  # xmax,ymin
        p3 = [ws[0][1], ws[1][1], ws[2][0]]  # xmax,ymax
        p4 = [ws[0][0], ws[1][1], ws[2][0]]  # xmin,ymax

        p.addUserDebugLine(p1, p2, lineColorRGB=[0, 0, 1], lineWidth=2.0, lifeTime=0, physicsClientId=self._physics_client_id)
        p.addUserDebugLine(p2, p3, lineColorRGB=[0, 0, 1], lineWidth=2.0, lifeTime=0, physicsClientId=self._physics_client_id)
        p.addUserDebugLine(p3, p4, lineColorRGB=[0, 0, 1], lineWidth=2.0, lifeTime=0,physicsClientId=self._physics_client_id)
        p.addUserDebugLine(p4, p1, lineColorRGB=[0, 0, 1], lineWidth=2.0, lifeTime=0, physicsClientId=self._physics_client_id)

        p.addUserDebugLine([0, 0, 0], [0.3, 0, 0], [1, 0, 0], parentObjectUniqueId=self.robot_id, parentLinkIndex=-1,
                           physicsClientId=self._physics_client_id)
        p.addUserDebugLine([0, 0, 0], [0, 0.3, 0], [0, 1, 0], parentObjectUniqueId=self.robot_id, parentLinkIndex=-1,
                           physicsClientId=self._physics_client_id)
        p.addUserDebugLine([0, 0, 0], [0, 0, 0.3], [0, 0, 1], parentObjectUniqueId=self.robot_id, parentLinkIndex=-1,
                           physicsClientId=self._physics_client_id)

        p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0], parentObjectUniqueId=self.robot_id,
                           parentLinkIndex=self.end_eff_idx, physicsClientId=self._physics_client_id)
        p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0], parentObjectUniqueId=self.robot_id,
                           parentLinkIndex=self.end_eff_idx, physicsClientId=self._physics_client_id)
        p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1], parentObjectUniqueId=self.robot_id,
                           parentLinkIndex=self.end_eff_idx, physicsClientId=self._physics_client_id) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def debug_gui(self):
        p.addUserDebugLine(self._tg_pose, [self._tg_pose[0] + 0.1, self._tg_pose[1], self._tg_pose[2]], [1, 0, 0],
                           physicsClientId=self._physics_client_id)
        p.addUserDebugLine(self._tg_pose, [self._tg_pose[0], self._tg_pose[1] + 0.1, self._tg_pose[2]], [0, 1, 0],
                           physicsClientId=self._physics_client_id)
        p.addUserDebugLine(self._tg_pose, [self._tg_pose[0], self._tg_pose[1], self._tg_pose[2] + 0.1], [0, 0, 1],
                           physicsClientId=self._physics_client_id) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def debug_gui(self):
        p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0], parentObjectUniqueId=self.obj_id, physicsClientId=self._physics_client_id)
        p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0], parentObjectUniqueId=self.obj_id, physicsClientId=self._physics_client_id)
        p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1], parentObjectUniqueId=self.obj_id, physicsClientId=self._physics_client_id) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def debug_gui(self):
        p.addUserDebugLine(self._target_pose, [self._target_pose[0] + 0.1, self._target_pose[1], self._target_pose[2]], [1, 0, 0],
                           physicsClientId=self._physics_client_id)
        p.addUserDebugLine(self._target_pose, [self._target_pose[0], self._target_pose[1] + 0.1, self._target_pose[2]], [0, 1, 0],
                           physicsClientId=self._physics_client_id)
        p.addUserDebugLine(self._target_pose, [self._target_pose[0], self._target_pose[1], self._target_pose[2] + 0.1], [0, 0, 1],
                           physicsClientId=self._physics_client_id) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def drawAABB(aabb):
	f = [aabbMin[0],aabbMin[1],aabbMin[2]]
	t = [aabbMax[0],aabbMin[1],aabbMin[2]]
	p.addUserDebugLine(f,t,[1,0,0])
	f = [aabbMin[0],aabbMin[1],aabbMin[2]]
	t = [aabbMin[0],aabbMax[1],aabbMin[2]]
	p.addUserDebugLine(f,t,[0,1,0])
	f = [aabbMin[0],aabbMin[1],aabbMin[2]]
	t = [aabbMin[0],aabbMin[1],aabbMax[2]]
	p.addUserDebugLine(f,t,[0,0,1])

	f = [aabbMin[0],aabbMin[1],aabbMax[2]]
	t = [aabbMin[0],aabbMax[1],aabbMax[2]]
	p.addUserDebugLine(f,t,[1,1,1])

	f = [aabbMin[0],aabbMin[1],aabbMax[2]]
	t = [aabbMax[0],aabbMin[1],aabbMax[2]]
	p.addUserDebugLine(f,t,[1,1,1])

	f = [aabbMax[0],aabbMin[1],aabbMin[2]]
	t = [aabbMax[0],aabbMin[1],aabbMax[2]]
	p.addUserDebugLine(f,t,[1,1,1])

	f = [aabbMax[0],aabbMin[1],aabbMin[2]]
	t = [aabbMax[0],aabbMax[1],aabbMin[2]]
	p.addUserDebugLine(f,t,[1,1,1])
	
	f = [aabbMax[0],aabbMax[1],aabbMin[2]]
	t = [aabbMin[0],aabbMax[1],aabbMin[2]]
	p.addUserDebugLine(f,t,[1,1,1])
	
	f = [aabbMin[0],aabbMax[1],aabbMin[2]]
	t = [aabbMin[0],aabbMax[1],aabbMax[2]]
	p.addUserDebugLine(f,t,[1,1,1])
	
	f = [aabbMax[0],aabbMax[1],aabbMax[2]]
	t = [aabbMin[0],aabbMax[1],aabbMax[2]]
	p.addUserDebugLine(f,t,[1.0,0.5,0.5])
	f = [aabbMax[0],aabbMax[1],aabbMax[2]]
	t = [aabbMax[0],aabbMin[1],aabbMax[2]]
	p.addUserDebugLine(f,t,[1,1,1])
	f = [aabbMax[0],aabbMax[1],aabbMax[2]]
	t = [aabbMax[0],aabbMax[1],aabbMin[2]]
	p.addUserDebugLine(f,t,[1,1,1]) 

# 需要导入模块: import pybullet [as 别名]
# 或者: from pybullet import addUserDebugLine [as 别名]
def _laserScan(self):
        """
        INTERNAL METHOD, a loop that simulate the laser and update the distance
        value of each laser
        """
        lastLidarTime = time.time()

        while not self._module_termination:
            nowLidarTime = time.time()

            if (nowLidarTime-lastLidarTime > 1/LASER_FRAMERATE):
                results = pybullet.rayTestBatch(
                    self.ray_from,
                    self.ray_to,
                    parentObjectUniqueId=self.robot_model,
                    parentLinkIndex=self.laser_id,
                    physicsClientId=self.physics_client)

                for i in range(NUM_RAY*len(ANGLE_LIST_POSITION)):
                    hitObjectUid = results[i][0]
                    hitFraction = results[i][2]
                    hitPosition = results[i][3]
                    self.laser_value[i] = hitFraction * RAY_LENGTH

                    if self.display:
                        if not self.ray_ids:
                            self._createDebugLine()

                        if (hitFraction == 1.):
                            pybullet.addUserDebugLine(
                                self.ray_from[i],
                                self.ray_to[i],
                                RAY_MISS_COLOR,
                                replaceItemUniqueId=self.ray_ids[i],
                                parentObjectUniqueId=self.robot_model,
                                parentLinkIndex=self.laser_id,
                                physicsClientId=self.physics_client)
                        else:  # pragma: no cover
                            localHitTo = [self.ray_from[i][0]+hitFraction*(
                                        self.ray_to[i][0]-self.ray_from[i][0]),
                                         self.ray_from[i][1]+hitFraction*(
                                        self.ray_to[i][1]-self.ray_from[i][1]),
                                         self.ray_from[i][2]+hitFraction*(
                                        self.ray_to[i][2]-self.ray_from[i][2])]
                            pybullet.addUserDebugLine(
                                self.ray_from[i],
                                localHitTo,
                                RAY_HIT_COLOR,
                                replaceItemUniqueId=self.ray_ids[i],
                                parentObjectUniqueId=self.robot_model,
                                parentLinkIndex=self.laser_id,
                                physicsClientId=self.physics_client)

                    else:
                        if self.ray_ids:
                            self._resetDebugLine()

                lastLidarTime = nowLidarTime