Python pybullet.addUserDebugLine() Examples
The following are 13
code examples of pybullet.addUserDebugLine().
You can vote up the ones you like or vote down the ones you don't like,
and go to the original project or source file by following the links above each example.
You may also want to check out all available functions/classes of the module
pybullet
, or try the search function
.
.
Example #1
| Source File: panda_env.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 6 votes |
|
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)
Example #2
| Source File: quadruped.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
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)
Example #3
| Source File: reset_dynamic_info.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
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)
Example #4
| Source File: kukaGymEnv.py From soccer-matlab with BSD 2-Clause "Simplified" License | 5 votes |
|
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
Example #5
| Source File: simulation.py From onshape-to-robot with MIT License | 5 votes |
|
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()
Example #6
| Source File: renderer.py From mvp_grasp with BSD 3-Clause "New" or "Revised" License | 5 votes |
|
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)
Example #7
| Source File: laser.py From qibullet with Apache License 2.0 | 5 votes |
|
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))
Example #8
| Source File: icub_env.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 5 votes |
|
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)
Example #9
| Source File: icub_push_gym_env.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 5 votes |
|
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)
Example #10
| Source File: world_env.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 5 votes |
|
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)
Example #11
| Source File: panda_push_gym_env.py From pybullet-robot-envs with GNU Lesser General Public License v2.1 | 5 votes |
|
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)
Example #12
| Source File: getAABB.py From soccer-matlab with BSD 2-Clause "Simplified" License | 4 votes |
|
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])
Example #13
| Source File: laser.py From qibullet with Apache License 2.0 | 4 votes |
|
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
