attach debug line to objects

[ahundt]

I'd like to propose tweaking how debug lines work, what do you think?

The modified version of #1258 below would hypothetically attach one third of the markers to the objects that fall, because the parent id is set to the spheres moving in the simulation. My hope is that the triad of debug lines attached to the objects would fall too, maintaining a constant relative pose offset from its parent object. In this case they would be moving with respect to the world frame.

Am I correct that something like this should work when attaching to robot links?

import pybullet as p
import time
import numpy as np

conid = p.connect(p.SHARED_MEMORY)
if (conid<0):
	p.connect(p.GUI)

p.setInternalSimFlags(0)
p.resetSimulation()


def createPoseMarker(position=np.array([0,0,0]),
                     orientation=np.array([0,0,0,1]),
                     x_color=np.array([1,0,0]),
                     y_color=np.array([0,1,0]),
                     z_color=np.array([0,0,1]),
                     lineLength=0.1,
                     lineWidth=1,
                     lifeTime=0,
                     parentObjectUniqueId=0,
                     parentLinkIndex=0,
                     physicsClientId=0):
    '''Create a pose marker that identifies a position and orientation in space with 3 colored lines.
    '''
    pts = np.array([[0,0,0],[lineLength,0,0],[0,lineLength,0],[0,0,lineLength]])
    rotIdentity = np.array([0,0,0,1])
    po, _ = p.multiplyTransforms(position, orientation, pts[0,:], rotIdentity)
    px, _ = p.multiplyTransforms(position, orientation, pts[1,:], rotIdentity)
    py, _ = p.multiplyTransforms(position, orientation, pts[2,:], rotIdentity)
    pz, _ = p.multiplyTransforms(position, orientation, pts[3,:], rotIdentity)
    p.addUserDebugLine(po, px, x_color, lineWidth, lifeTime, parentObjectUniqueId, parentLinkIndex, physicsClientId)
    p.addUserDebugLine(po, py, y_color, lineWidth, lifeTime, parentObjectUniqueId, parentLinkIndex, physicsClientId)
    p.addUserDebugLine(po, pz, z_color, lineWidth, lifeTime, parentObjectUniqueId, parentLinkIndex, physicsClientId)

p.loadURDF("plane.urdf",useMaximalCoordinates=True)
p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True)

gravXid = p.addUserDebugParameter("gravityX",-10,10,0)
gravYid = p.addUserDebugParameter("gravityY",-10,10,0)
gravZid = p.addUserDebugParameter("gravityZ",-10,10,-10)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
l = 0
for i in range (4):
    for j in range (4):
        for k in range (4):
            location = np.array([0.02*i,0.02*j,0.2+0.02*k])
            ob = p.loadURDF("sphere_1cm.urdf", location, useMaximalCoordinates=True)
            orientation = p.getQuaternionFromEuler(location * 200)
            if (l % 3 is not 0 ):
                ob = 0
            createPoseMarker(location, orientation, parentObjectUniqueId=ob)
            l += 1
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
while True:
    gravX = p.readUserDebugParameter(gravXid)
    gravY = p.readUserDebugParameter(gravYid)
    gravZ = p.readUserDebugParameter(gravZid)
    p.setGravity(gravX,gravY,gravZ)
    time.sleep(0.01)

[erwincoumans]

Debug lines that have a valid parent object unique id will follow the parent object. Isn't that working. By the way, ob = 0 is likely still a valid parent object uid, use -1 for an invalid value (= attached to world), or explicitly name the arguments and don't provide a parent uid.

[ahundt]

Debug lines that have a valid parent object unique id will follow the parent object. Isn't that working.

I think it might only be working for links, not for any arbitrary object.

use -1 for an invalid value

Could the actual default values of parameters be added throughout the pybullet.org docs?

I looked for that default but could't figure it out from the docs so I guessed since it is a low impact first implementation, I'll fix it now. Also what was default Quaternion/rpy value for text? That wasn't obvious.

The reason to document defaults is for functions like createPoseMarker, because the actual default values are critical for anyone implementing APIs that in turn must call the pybullet API. Right now the docs only have the default behavior but not values.