def draw_ground_truth(coord_seq, frame, duration, shift):
global joint_info
joint = coord_seq[frame]
shift = np.array(shift)
for i in range(1, 17):
# print(x[11], x[14])
joint_fa = joint_info['father'][i]
if joint_info['side'][i] == 'right':
p.addUserDebugLine(lineFromXYZ=joint[i] + shift,
lineToXYZ=joint[joint_fa] + shift,
lineColorRGB=(255, 0, 0),
lineWidth=1,
lifeTime=duration)
else:
p.addUserDebugLine(lineFromXYZ=joint[i] + shift,
lineToXYZ=joint[joint_fa] + shift,
lineColorRGB=(0, 0, 0),
lineWidth=1,
lifeTime=duration)
while True:
events = p.getVREvents(p.VR_DEVICE_HMD + p.VR_DEVICE_GENERIC_TRACKER)
for e in (events):
pos = e[POSITION]
mat = p.getMatrixFromQuaternion(e[ORIENTATION])
dir0 = [mat[0], mat[3], mat[6]]
dir1 = [mat[1], mat[4], mat[7]]
dir2 = [mat[2], mat[5], mat[8]]
lineLen = 0.1
dir = [-mat[2], -mat[5], -mat[8]]
to = [pos[0] + lineLen * dir[0], pos[1] + lineLen * dir[1], pos[2] + lineLen * dir[2]]
toX = [pos[0] + lineLen * dir0[0], pos[1] + lineLen * dir0[1], pos[2] + lineLen * dir0[2]]
toY = [pos[0] + lineLen * dir1[0], pos[1] + lineLen * dir1[1], pos[2] + lineLen * dir1[2]]
toZ = [pos[0] + lineLen * dir2[0], pos[1] + lineLen * dir2[1], pos[2] + lineLen * dir2[2]]
p.addUserDebugLine(pos, toX, [1, 0, 0], 1)
p.addUserDebugLine(pos, toY, [0, 1, 0], 1)
p.addUserDebugLine(pos, toZ, [0, 0, 1], 1)
p.addUserDebugLine(pos, to, [0.5, 0.5, 0.], 1, 3)
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED):
prevPosition[e[CONTROLLER_ID]] = e[POSITION]
if (e[BUTTONS][32] & p.VR_BUTTON_WAS_TRIGGERED):
widths[e[CONTROLLER_ID]] = widths[e[0]] + 1
if (widths[e[CONTROLLER_ID]] > 20):
widths[e[CONTROLLER_ID]] = 1
if (e[BUTTONS][1] & p.VR_BUTTON_WAS_TRIGGERED):
if (useRealTimeSimulation):
dt = datetime.now()
t = (dt.second / 60.) * 2. * math.pi
else:
t = t + 0.01
time.sleep(0.01)
for i in range(1):
pos = [2. * math.cos(t), 2. * math.cos(t), 0. + 2. * math.sin(t)]
jointPoses = p.calculateInverseKinematics(sawyerId,
sawyerEndEffectorIndex,
pos,
jointDamping=jd,
solver=ikSolver,
maxNumIterations=100)
#reset the joint state (ignoring all dynamics, not recommended to use during simulation)
for i in range(numJoints):
jointInfo = p.getJointInfo(sawyerId, i)
qIndex = jointInfo[3]
if qIndex > -1:
p.resetJointState(sawyerId, i, jointPoses[qIndex - 7])
ls = p.getLinkState(sawyerId, sawyerEndEffectorIndex)
if (hasPrevPose):
p.addUserDebugLine(prevPose, pos, [0, 0, 0.3], 1, trailDuration)
p.addUserDebugLine(prevPose1, ls[4], [1, 0, 0], 1, trailDuration)
prevPose = pos
prevPose1 = ls[4]
hasPrevPose = 1
baseOrientationB = p.getQuaternionFromEuler([0, 0.3, 0]) #[0,0.5,0.5,0]
basePositionB = [1.5, 0, 1]
obA = -1
obB = -1
obA = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=geom, basePosition=[0.5, 0, 1])
obB = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=geomBox,
basePosition=basePositionB,
baseOrientation=baseOrientationB)
lineWidth = 3
colorRGB = [1, 0, 0]
lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0],
lineToXYZ=[0, 0, 0],
lineColorRGB=colorRGB,
lineWidth=lineWidth,
lifeTime=0)
pitch = 0
yaw = 0
while (p.isConnected()):
pitch += 0.01
if (pitch >= 3.1415 * 2.):
pitch = 0
yaw += 0.01
if (yaw >= 3.1415 * 2.):
yaw = 0
baseOrientationB = p.getQuaternionFromEuler([yaw, pitch, 0])
if (obB >= 0):
import pybullet as p
import time
cid = p.connect(p.SHARED_MEMORY)
if (cid<0):
p.connect(p.GUI)
p.loadURDF("plane.urdf")
kuka = p.loadURDF("kuka_iiwa/model.urdf")
p.addUserDebugText("tip", [0,0,0.1],textColorRGB=[1,0,0],textSize=1.5,parentObjectUniqueId=kuka, parentLinkIndex=6)
p.addUserDebugLine([0,0,0],[0.1,0,0],[1,0,0],parentObjectUniqueId=kuka, parentLinkIndex=6)
p.addUserDebugLine([0,0,0],[0,0.1,0],[0,1,0],parentObjectUniqueId=kuka, parentLinkIndex=6)
p.addUserDebugLine([0,0,0],[0,0,0.1],[0,0,1],parentObjectUniqueId=kuka, parentLinkIndex=6)
p.setRealTimeSimulation(0)
angle=0
while (True):
time.sleep(0.01)
p.resetJointState(kuka,2,angle)
p.resetJointState(kuka,3,angle)
angle+=0.01
numRays = 1024 rayLen = 13 rayHitColor = [1,0,0] rayMissColor = [0,1,0] replaceLines = True for i in range (numRays): rayFrom.append([0,0,1]) rayTo.append([rayLen*math.sin(2.*math.pi*float(i)/numRays), rayLen*math.cos(2.*math.pi*float(i)/numRays),1]) if (replaceLines): rayIds.append(p.addUserDebugLine(rayFrom[i], rayTo[i], rayMissColor)) else: rayIds.append(-1) if (not useGui): timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json") numSteps = 10 if (useGui): numSteps = 327680 for i in range (numSteps): p.stepSimulation() for j in range (8): results = p.rayTestBatch(rayFrom,rayTo,j+1)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
visualShapeId = p.createVisualShape(shapeType=p.GEOM_SPHERE, rgbaColor=[1,1,1,1], radius=0.03 )
collisionShapeId = -1 #p.createCollisionShape(shapeType=p.GEOM_MESH, fileName="duck_vhacd.obj", collisionFramePosition=shift,meshScale=meshScale)
for i in range (4):
w = cornersX[i]
h = cornersY[i]
rayFrom,rayTo, _= getRayFromTo(w,h)
rf = np.array(rayFrom)
rt = np.array(rayTo)
vec = rt-rf
l = np.sqrt(np.dot(vec,vec))
newTo = (0.01/l)*vec+rf
#print("len vec=",np.sqrt(np.dot(vec,vec)))
p.addUserDebugLine(rayFrom,newTo,[1,0,0])
corners3D.append(newTo)
count = 0
stepX=5
stepY=5
for w in range(0,imgW,stepX):
for h in range (0,imgH,stepY):
count+=1
if ((count % 100)==0):
print(count,"out of ", imgW*imgH/(stepX*stepY))
rayFrom,rayTo, alpha = getRayFromTo(w*(width/imgW),h*(height/imgH))
rf = np.array(rayFrom)
rt = np.array(rayTo)
vec = rt-rf
l = np.sqrt(np.dot(vec,vec))
pos[0] + lineLen * dir[0], pos[1] + lineLen * dir[1],
pos[2] + lineLen * dir[2]
]
toX = [
pos[0] + lineLen * dir0[0], pos[1] + lineLen * dir0[1],
pos[2] + lineLen * dir0[2]
]
toY = [
pos[0] + lineLen * dir1[0], pos[1] + lineLen * dir1[1],
pos[2] + lineLen * dir1[2]
]
toZ = [
pos[0] + lineLen * dir2[0], pos[1] + lineLen * dir2[1],
pos[2] + lineLen * dir2[2]
]
p.addUserDebugLine(pos, toX, [1, 0, 0], 1)
p.addUserDebugLine(pos, toY, [0, 1, 0], 1)
p.addUserDebugLine(pos, toZ, [0, 0, 1], 1)
p.addUserDebugLine(pos, to, [0.5, 0.5, 0.], 1, 3)
events = p.getVREvents()
for e in (events):
if (e[BUTTONS][33] & p.VR_BUTTON_WAS_TRIGGERED):
prevPosition[e[CONTROLLER_ID]] = e[POSITION]
if (e[BUTTONS][32] & p.VR_BUTTON_WAS_TRIGGERED):
widths[e[CONTROLLER_ID]] = widths[e[0]] + 1
if (widths[e[CONTROLLER_ID]] > 20):
widths[e[CONTROLLER_ID]] = 1
if (e[BUTTONS][1] & p.VR_BUTTON_WAS_TRIGGERED):
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)
print(info)
color = [0, 0, 1]
"""Joint Indeces:
8: right thigh
10: right shin
15: left thigh
17: left shin
21: middle of upper right arm
23: right elbow
26: middle of upper left arm
28: left elbow
"""
p.addUserDebugLine([0, 0, 0], [0, 0, 1],
color,
3,
parentObjectUniqueId=ob,
parentLinkIndex=28)
p.applyExternalForce(objectUniqueId=1,
linkIndex=28,
forceObj=[100, 100, 100],
posObj=[0, 0, 0],
flags=2)
p.setGravity(0, 0, -9.81)
# p.setTimeStep(-0.0001)
p.setRealTimeSimulation(0)
p.setJointMotorControl2(human, 25, p.POSITION_CONTROL)
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)
delta_val = -DELTA_STEP_SIZE
print("Stepping backward along {} axis".format(axis[1]))
else:
delta_val = DELTA_STEP_SIZE
print("Stepping forward along {} axis".format(axis))
path = Line(env.robot_interface.ee_pose,
num_pts=NUM_STEPS,
delta_val=delta_val,
dim=axis_num % 3)
if env.world.is_sim:
# Add a marker to visualize path in pybullet
goal_marker_id = pb.addUserDebugLine(
path.path[0][:3],
path.path[-1][:3], [0, 0, 1.0],
lineWidth=5.0,
physicsClientId=env.world.physics_id)
done = False
goal_exceeds_boundary = False
while not done and step_num < NUM_STEPS:
start = time.time()
action = get_next_action(observation['ee_position'], path.path,
step_num)
# Alert if goal exceeds boundary box set by safenet.
if exceeds_boundaries(path.path[step_num][:3],
env.robot_interface.safenet_ee_pos_upper,
env.robot_interface.safenet_ee_pos_lower,
#trailDuration is duration (in seconds) after debug lines will be removed automatically #use 0 for no-removal trailDuration = 1 while 1: if (useRealTimeSimulation): dt = datetime.now() t = (dt.second/60.)*2.*math.pi else: t=t+0.01 time.sleep(0.01) for i in range (1): pos = [2.*math.cos(t),2.*math.cos(t),0.+2.*math.sin(t)] jointPoses = p.calculateInverseKinematics(sawyerId,sawyerEndEffectorIndex,pos,jointDamping=jd,solver=ikSolver) #reset the joint state (ignoring all dynamics, not recommended to use during simulation) for i in range (numJoints): jointInfo = p.getJointInfo(sawyerId, i) qIndex = jointInfo[3] if qIndex > -1: p.resetJointState(sawyerId,i,jointPoses[qIndex-7]) ls = p.getLinkState(sawyerId,sawyerEndEffectorIndex) if (hasPrevPose): p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration) p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration) prevPose=pos prevPose1=ls[4] hasPrevPose = 1
obA = -1
obB = -1
obA = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=geom,
basePosition=[0.5, 0, 1])
obB = p.createMultiBody(baseMass=0,
baseCollisionShapeIndex=geomBox,
basePosition=basePositionB,
baseOrientation=baseOrientationB)
lineWidth = 3
colorRGB = [1, 0, 0]
lineId = p.addUserDebugLine(lineFromXYZ=[0, 0, 0],
lineToXYZ=[0, 0, 0],
lineColorRGB=colorRGB,
lineWidth=lineWidth,
lifeTime=0)
pitch = 0
yaw = 0
while (p.isConnected()):
pitch += 0.01
if (pitch >= 3.1415 * 2.):
pitch = 0
yaw += 0.01
if (yaw >= 3.1415 * 2.):
yaw = 0
baseOrientationB = p.getQuaternionFromEuler([yaw, pitch, 0])
if (obB >= 0):
for e in (events):
if (e[CONTROLLER_ID]==controllerId ):
for a in range(10):
print("analog axis"+str(a)+"="+str(e[8][a]))
if (e[BUTTONS][33]&p.VR_BUTTON_WAS_TRIGGERED):
prevPosition[e[CONTROLLER_ID]] = e[POSITION]
if (e[BUTTONS][32]&p.VR_BUTTON_WAS_TRIGGERED):
widths[e[CONTROLLER_ID]]=widths[e[0]]+1
if (widths[e[CONTROLLER_ID]]>20):
widths[e[CONTROLLER_ID]] = 1
if (e[BUTTONS][1]&p.VR_BUTTON_WAS_TRIGGERED):
p.resetSimulation()
#p.setGravity(0,0,-10)
p.removeAllUserDebugItems()
p.loadURDF("plane.urdf")
if (e[BUTTONS][33]==p.VR_BUTTON_IS_DOWN):
pt = prevPosition[e[CONTROLLER_ID]]
#print(prevPosition[e[0]])
print("e[POSITION]")
print(e[POSITION])
print("pt")
print(pt)
diff = [pt[0]-e[POSITION][0],pt[1]-e[POSITION][1],pt[2]-e[POSITION][2]]
lenSqr = diff[0]*diff[0]+diff[1]*diff[1]+diff[2]*diff[2]
ptDistThreshold = 0.01
if (lenSqr>(ptDistThreshold*ptDistThreshold)):
p.addUserDebugLine(e[POSITION],prevPosition[e[CONTROLLER_ID]],colors[e[CONTROLLER_ID]],widths[e[CONTROLLER_ID]])
#p.loadURDF("cube_small.urdf",e[1])
colors[e[CONTROLLER_ID]] = [1-colors[e[CONTROLLER_ID]][0],1-colors[e[CONTROLLER_ID]][1],1-colors[e[CONTROLLER_ID]][2]]
prevPosition[e[CONTROLLER_ID]] = e[POSITION]
p.configureDebugVisualizer(p.COV_ENABLE_GUI,0)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)#disable this to make it faster
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0)
p.setPhysicsEngineParameter(enableConeFriction=1)
for i in range (num):
print("progress:",i,num)
x = velocity*math.sin(2.*3.1415*float(i)/num)
y = velocity*math.cos(2.*3.1415*float(i)/num)
print("velocity=",x,y)
sphere=p.loadURDF("sphere_small_zeroinertia.urdf", flags=p.URDF_USE_INERTIA_FROM_FILE, useMaximalCoordinates=useMaximalCoordinates)
p.changeDynamics(sphere,-1,lateralFriction=0.02)
#p.changeDynamics(sphere,-1,rollingFriction=10)
p.changeDynamics(sphere,-1,linearDamping=0)
p.changeDynamics(sphere,-1,angularDamping=0)
p.resetBaseVelocity(sphere,linearVelocity=[x,y,0])
prevPos = [0,0,0]
for i in range (2048):
p.stepSimulation()
pos = p.getBasePositionAndOrientation(sphere)[0]
if (i&64):
p.addUserDebugLine(prevPos,pos,[1,0,0],1)
prevPos = pos
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
while (1):
time.sleep(0.01)
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])
def loadDebugRefFrames(self):
p.addUserDebugText("rightpad", [0, 0, 0.05],
textColorRGB=[1, 0, 0],
textSize=1.,
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.rightFingerPadIndex)
p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.rightFingerPadIndex)
p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.rightFingerPadIndex)
p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.rightFingerPadIndex)
p.addUserDebugText("leftpad", [0, 0, 0.05],
textColorRGB=[1, 0, 0],
textSize=1.,
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.leftFingerPadIndex)
p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.leftFingerPadIndex)
p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.leftFingerPadIndex)
p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.leftFingerPadIndex)
p.addUserDebugText("wrist", [0, 0, 0.05],
textColorRGB=[1, 0, 0],
textSize=1.,
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.wristWIndex)
p.addUserDebugLine([0, 0, 0], [0.1, 0, 0], [1, 0, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.wristWIndex)
p.addUserDebugLine([0, 0, 0], [0, 0.1, 0], [0, 1, 0],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.wristWIndex)
p.addUserDebugLine([0, 0, 0], [0, 0, 0.1], [0, 0, 1],
parentObjectUniqueId=self.gripperUid,
parentLinkIndex=self.wristWIndex)