def initializeFromBulletBody(self, bodyUid, physicsClientId):
		self.initialize()

		#always create a base link
		baseLink = UrdfLink()
		baseLinkIndex = -1
		self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink, physicsClientId)
		baseLink.link_name = 	p.getBodyInfo(bodyUid, physicsClientId=physicsClientId)[0].decode("utf-8") 		
		self.linkNameToIndex[baseLink.link_name]=len(self.urdfLinks)
		self.urdfLinks.append(baseLink)
		
	
		#print(visualShapes)
		#optionally create child links and joints
		for j in range(p.getNumJoints(bodyUid,physicsClientId=physicsClientId)):
			jointInfo = p.getJointInfo(bodyUid,j,physicsClientId=physicsClientId)
			urdfLink = UrdfLink()
			self.convertLinkFromMultiBody(bodyUid, j, urdfLink,physicsClientId)
			urdfLink.link_name = jointInfo[12].decode("utf-8")
			self.linkNameToIndex[urdfLink.link_name]=len(self.urdfLinks)
			self.urdfLinks.append(urdfLink)
	
			urdfJoint = UrdfJoint()
			urdfJoint.link = urdfLink
			urdfJoint.joint_name = jointInfo[1].decode("utf-8")
			urdfJoint.joint_type = jointInfo[2]
			urdfJoint.joint_axis_xyz = jointInfo[13]
			orgParentIndex = jointInfo[16]
			if (orgParentIndex<0):
				urdfJoint.parent_name = baseLink.link_name
			else:
				parentJointInfo = p.getJointInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
				urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
			urdfJoint.child_name = urdfLink.link_name

			#todo, compensate for inertia/link frame offset
			
			dynChild = p.getDynamicsInfo(bodyUid,j,physicsClientId=physicsClientId)
			childInertiaPos = dynChild[3]
			childInertiaOrn = dynChild[4]
			parentCom2JointPos=jointInfo[14] 
			parentCom2JointOrn=jointInfo[15]
			tmpPos,tmpOrn = p.multiplyTransforms(childInertiaPos,childInertiaOrn,parentCom2JointPos,parentCom2JointOrn)
			tmpPosInv,tmpOrnInv = p.invertTransform(tmpPos,tmpOrn)
			dynParent = p.getDynamicsInfo(bodyUid,orgParentIndex,physicsClientId=physicsClientId)
			parentInertiaPos = dynParent[3]
			parentInertiaOrn = dynParent[4]
			
			pos,orn = p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, tmpPosInv, tmpOrnInv)
			pos,orn_unused=p.multiplyTransforms(parentInertiaPos,parentInertiaOrn, parentCom2JointPos,[0,0,0,1])
			
			urdfJoint.joint_origin_xyz = pos
			urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
			
			self.urdfJoints.append(urdfJoint)

	def __init__(self, joint_name, bodies, bodyIndex, jointIndex):
		self.bodies = bodies
		self.bodyIndex = bodyIndex
		self.jointIndex = jointIndex
		self.joint_name = joint_name
		_,_,_,_,_,_,_,_,self.lowerLimit, self.upperLimit,_,_,_ = p.getJointInfo(self.bodies[self.bodyIndex], self.jointIndex)
		self.power_coeff = 0

 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)

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

def multiplyJacobian(robot, jacobian, vector):
	result = [0.0, 0.0, 0.0]
	i = 0
	for c in range(len(vector)):
		if p.getJointInfo(robot, c)[3] > -1:
			for r in range(3):
				result[r] += jacobian[r][i] * vector[c]
			i += 1
	return result

	def addToScene(self, bodies):
		if self.parts is not None:
			parts = self.parts
		else:
			parts = {}

		if self.jdict is not None:
			joints = self.jdict
		else:
			joints = {}

		if self.ordered_joints is not None:
			ordered_joints = self.ordered_joints
		else:
			ordered_joints = []

		dump = 0
		for i in range(len(bodies)):
			if p.getNumJoints(bodies[i]) == 0:
				part_name, robot_name = p.getBodyInfo(bodies[i], 0)
				robot_name = robot_name.decode("utf8")
				part_name = part_name.decode("utf8")
				parts[part_name] = BodyPart(part_name, bodies, i, -1)
			for j in range(p.getNumJoints(bodies[i])):
				p.setJointMotorControl2(bodies[i],j,p.POSITION_CONTROL,positionGain=0.1,velocityGain=0.1,force=0)
				_,joint_name,_,_,_,_,_,_,_,_,_,_,part_name = p.getJointInfo(bodies[i], j)

				joint_name = joint_name.decode("utf8")
				part_name = part_name.decode("utf8")

				if dump: print("ROBOT PART '%s'" % part_name)
				if dump: print("ROBOT JOINT '%s'" % joint_name) # limits = %+0.2f..%+0.2f effort=%0.3f speed=%0.3f" % ((joint_name,) + j.limits()) )

				parts[part_name] = BodyPart(part_name, bodies, i, j)

				if part_name == self.robot_name:
					self.robot_body = parts[part_name]

				if i == 0 and j == 0 and self.robot_body is None:  # if nothing else works, we take this as robot_body
					parts[self.robot_name] = BodyPart(self.robot_name, bodies, 0, -1)
					self.robot_body = parts[self.robot_name]

				if joint_name[:6] == "ignore":
					Joint(joint_name, bodies, i, j).disable_motor()
					continue

				if joint_name[:8] != "jointfix":
					joints[joint_name] = Joint(joint_name, bodies, i, j)
					ordered_joints.append(joints[joint_name])

					joints[joint_name].power_coef = 100.0

		return parts, joints, ordered_joints, self.robot_body

 def reset(self):
   self.quadruped = p.loadURDF("quadruped/quadruped.urdf",0,0,.3)
   self.kp = 1
   self.kd = 0.1
   self.maxForce = 100
   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()

def Step(stepIndex):
	for objectId in range(objectNum):
		record = log[stepIndex*objectNum+objectId]
		Id = record[2]
		pos = [record[3],record[4],record[5]]
		orn = [record[6],record[7],record[8],record[9]]
		p.resetBasePositionAndOrientation(Id,pos,orn)
		numJoints = p.getNumJoints(Id)
		for i in range (numJoints):
			jointInfo = p.getJointInfo(Id,i)
			qIndex = jointInfo[3]
			if qIndex > -1:
				p.resetJointState(Id,i,record[qIndex-7+17])

  def reset(self):
    self.initial_z = None
   
    objs = p.loadMJCF(os.path.join(self.urdfRootPath,"mjcf/humanoid_symmetric_no_ground.xml"),flags = p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS)
    self.human = objs[0]
    self.jdict = {}
    self.ordered_joints = []
    self.ordered_joint_indices = []

    for j in range( p.getNumJoints(self.human) ):
      info = p.getJointInfo(self.human, j)
      link_name = info[12].decode("ascii")
      if link_name=="left_foot": self.left_foot = j
      if link_name=="right_foot": self.right_foot = j
      self.ordered_joint_indices.append(j)
      if info[2] != p.JOINT_REVOLUTE: continue
      jname = info[1].decode("ascii")
      self.jdict[jname] = j
      lower, upper = (info[8], info[9])
      self.ordered_joints.append( (j, lower, upper) )
      p.setJointMotorControl2(self.human, j, controlMode=p.VELOCITY_CONTROL, force=0)

    self.motor_names  = ["abdomen_z", "abdomen_y", "abdomen_x"]
    self.motor_power  = [100, 100, 100]
    self.motor_names += ["right_hip_x", "right_hip_z", "right_hip_y", "right_knee"]
    self.motor_power += [100, 100, 300, 200]
    self.motor_names += ["left_hip_x", "left_hip_z", "left_hip_y", "left_knee"]
    self.motor_power += [100, 100, 300, 200]
    self.motor_names += ["right_shoulder1", "right_shoulder2", "right_elbow"]
    self.motor_power += [75, 75, 75]
    self.motor_names += ["left_shoulder1", "left_shoulder2", "left_elbow"]
    self.motor_power += [75, 75, 75]
    self.motors = [self.jdict[n] for n in self.motor_names]
    print("self.motors")
    print(self.motors)
    print("num motors")
    print(len(self.motors))

p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)


jointNamesToIndex={}


p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
vision = p.loadURDF("vision60.urdf",[0,0,0.4],useFixedBase=False)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)


for j in range(p.getNumJoints(vision)):
	jointInfo = p.getJointInfo(vision,j)
	jointInfoName = jointInfo[1].decode("utf-8") 
	print("joint ",j," = ",jointInfoName, "type=",jointTypeNames[jointInfo[2]])
	jointNamesToIndex[jointInfoName ]=j
	#print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName])
	p.setJointMotorControl2(vision,j,p.VELOCITY_CONTROL,targetVelocity=0, force=jointFriction)


chassis_right_center = jointNamesToIndex['chassis_right_center']
motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_joint']
motor_front_rightS_joint = jointNamesToIndex['motor_front_rightS_joint']
hip_front_rightR_joint = jointNamesToIndex['hip_front_rightR_joint']
knee_front_rightR_joint = jointNamesToIndex['knee_front_rightR_joint']
motor_front_rightL_joint = jointNamesToIndex['motor_front_rightL_joint']
motor_back_rightR_joint = jointNamesToIndex['motor_back_rightR_joint']
motor_back_rightS_joint = jointNamesToIndex['motor_back_rightS_joint']

]


def euc_dist(posA, posB):
  dist = 0.
  for i in range(len(posA)):
    dist += (posA[i] - posB[i])**2
  return dist


p.setRealTimeSimulation(1)

controllers = [e[0] for e in p.getVREvents()]

for j in range(p.getNumJoints(kuka_gripper)):
  print(p.getJointInfo(kuka_gripper, j))
while True:

  events = p.getVREvents()
  for e in (events):

    # Only use one controller
    ###########################################
    # This is important: make sure there's only one VR Controller!
    if e[0] == controllers[0]:
      break

    sq_len = euc_dist(p.getLinkState(kuka, 6)[0], e[POSITION])

    # A simplistic version of gripper control
    #@TO-DO: Add slider for the gripper

#a mimic joint can act as a gear between two joints
#you can control the gear ratio in magnitude and sign (>0 reverses direction)

import pybullet as p
import time
p.connect(p.GUI)
p.loadURDF("plane.urdf", 0, 0, -2)
wheelA = p.loadURDF("differential/diff_ring.urdf", [0, 0, 0])
for i in range(p.getNumJoints(wheelA)):
  print(p.getJointInfo(wheelA, i))
  p.setJointMotorControl2(wheelA, i, p.VELOCITY_CONTROL, targetVelocity=0, force=0)

c = p.createConstraint(wheelA,
                       1,
                       wheelA,
                       3,
                       jointType=p.JOINT_GEAR,
                       jointAxis=[0, 1, 0],
                       parentFramePosition=[0, 0, 0],
                       childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=1, maxForce=10000)

c = p.createConstraint(wheelA,
                       2,
                       wheelA,
                       4,
                       jointType=p.JOINT_GEAR,
                       jointAxis=[0, 1, 0],
                       parentFramePosition=[0, 0, 0],
                       childFramePosition=[0, 0, 0])
p.changeConstraint(c, gearRatio=-1, maxForce=10000)

if (cid<0):
	p.connect(p.GUI)
	
p.resetSimulation()
p.setGravity(0,0,-10)
useRealTimeSim = 1

#for video recording (works best on Mac and Linux, not well on Windows)
#p.startStateLogging(p.STATE_LOGGING_VIDEO_MP4, "racecar.mp4")
p.setRealTimeSimulation(useRealTimeSim) # either this
p.loadURDF("plane.urdf")
#p.loadSDF("stadium.sdf")

car = p.loadURDF("racecar/racecar_differential.urdf") #, [0,0,2],useFixedBase=True)
for i in range (p.getNumJoints(car)):
	print (p.getJointInfo(car,i))
for wheel in range(p.getNumJoints(car)):
		p.setJointMotorControl2(car,wheel,p.VELOCITY_CONTROL,targetVelocity=0,force=0)
		p.getJointInfo(car,wheel)	

wheels = [8,15]
print("----------------")

#p.setJointMotorControl2(car,10,p.VELOCITY_CONTROL,targetVelocity=1,force=10)
c = p.createConstraint(car,9,car,11,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=1, maxForce=10000)

c = p.createConstraint(car,10,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-1, maxForce=10000)

c = p.createConstraint(car,9,car,13,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])

  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

import pybullet as p
import time
import math
from datetime import datetime
from datetime import datetime

clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
	p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
husky = p.loadURDF("husky/husky.urdf",[0.290388,0.329902,-0.310270],[0.002328,-0.000984,0.996491,0.083659])
for i in range (p.getNumJoints(husky)):
	print(p.getJointInfo(husky,i))
kukaId  = p.loadURDF("kuka_iiwa/model_free_base.urdf", 0.193749,0.345564,0.120208,0.002327,-0.000988,0.996491,0.083659)
ob = kukaId
jointPositions=[ 3.559609, 0.411182, 0.862129, 1.744441, 0.077299, -1.129685, 0.006001 ]
for jointIndex in range (p.getNumJoints(ob)):
	p.resetJointState(ob,jointIndex,jointPositions[jointIndex])


#put kuka on top of husky

cid = p.createConstraint(husky,-1,kukaId,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0.,0.,-.5],[0,0,0,1])


baseorn = p.getQuaternionFromEuler([3.1415,0,0.3])
baseorn = [0,0,0,1]
#[0, 0, 0.707, 0.707]

#p.resetBasePositionAndOrientation(kukaId,[0,0,0],baseorn)#[0,0,0,1])
kukaEndEffectorIndex = 6

                              linkOrientations=linkOrientations,
                              linkInertialFramePositions=linkInertialFramePositions,
                              linkInertialFrameOrientations=linkInertialFrameOrientations,
                              linkParentIndices=indices,
                              linkJointTypes=jointTypes,
                              linkJointAxis=axis,
                              useMaximalCoordinates=useMaximalCoordinates)

p.setGravity(0, 0, -10)
p.setRealTimeSimulation(0)

anistropicFriction = [1, 0.01, 0.01]
p.changeDynamics(sphereUid, -1, lateralFriction=2, anisotropicFriction=anistropicFriction)
p.getNumJoints(sphereUid)
for i in range(p.getNumJoints(sphereUid)):
  p.getJointInfo(sphereUid, i)
  p.changeDynamics(sphereUid, i, lateralFriction=2, anisotropicFriction=anistropicFriction)

dt = 1. / 240.
SNAKE_NORMAL_PERIOD = 0.1  #1.5
m_wavePeriod = SNAKE_NORMAL_PERIOD

m_waveLength = 4
m_wavePeriod = 1.5
m_waveAmplitude = 0.4
m_waveFront = 0.0
#our steering value
m_steering = 0.0
m_segmentLength = sphereRadius * 2.0
forward = 0

p.setGravity(0, 0, 0)
p.setTimeStep(fixedTimeStep)

orn = p.getQuaternionFromEuler([0, 0, 0.4])
p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur_v1.urdf", [1, -1, .3],
                       orn,
                       useFixedBase=False,
                       useMaximalCoordinates=useMaximalCoordinates,
                       flags=p.URDF_USE_IMPLICIT_CYLINDER)
nJoints = p.getNumJoints(quadruped)

jointNameToId = {}
for i in range(nJoints):
  jointInfo = p.getJointInfo(quadruped, i)
  jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]

motor_front_rightR_joint = jointNameToId['motor_front_rightR_joint']
motor_front_rightL_joint = jointNameToId['motor_front_rightL_joint']
knee_front_rightL_link = jointNameToId['knee_front_rightL_link']
hip_front_rightR_link = jointNameToId['hip_front_rightR_link']
knee_front_rightR_link = jointNameToId['knee_front_rightR_link']
motor_front_rightL_link = jointNameToId['motor_front_rightL_link']
motor_front_leftR_joint = jointNameToId['motor_front_leftR_joint']
hip_front_leftR_link = jointNameToId['hip_front_leftR_link']
knee_front_leftR_link = jointNameToId['knee_front_leftR_link']
motor_front_leftL_joint = jointNameToId['motor_front_leftL_joint']
motor_front_leftL_link = jointNameToId['motor_front_leftL_link']
knee_front_leftL_link = jointNameToId['knee_front_leftL_link']
motor_back_rightR_joint = jointNameToId['motor_back_rightR_joint']

import pybullet as p
p.connect(p.GUI)
r2d2 = p.loadURDF("r2d2.urdf", [0, 0, 1])
for l in range(p.getNumJoints(r2d2)):
  print(p.getJointInfo(r2d2, l))

p.loadURDF("r2d2.urdf", [2, 0, 1])
p.loadURDF("r2d2.urdf", [4, 0, 1])

p.getCameraImage(320, 200, flags=p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX)
segLinkIndex = 1
verbose = 0

while (1):
  keys = p.getKeyboardEvents()
  #for k in keys:
  #	print("key=",k,"state=", keys[k])
  if ord('1') in keys:
    state = keys[ord('1')]
    if (state & p.KEY_WAS_RELEASED):
      verbose = 1 - verbose
  if ord('s') in keys:
    state = keys[ord('s')]
    if (state & p.KEY_WAS_RELEASED):
      segLinkIndex = 1 - segLinkIndex
      #print("segLinkIndex=",segLinkIndex)
  flags = 0
  if (segLinkIndex):
    flags = p.ER_SEGMENTATION_MASK_OBJECT_AND_LINKINDEX

  img = p.getCameraImage(320, 200, flags=flags)

p.setAdditionalSearchPath(pd.getDataPath())

plane = p.loadURDF("plane.urdf")
p.setGravity(0, 0, -9.8)
p.setTimeStep(1. / 500)
#p.setDefaultContactERP(0)
#urdfFlags = p.URDF_USE_SELF_COLLISION+p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS
urdfFlags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF("laikago/laikago.urdf", [0, 0, .5], [0, 0.5, 0.5, 0],
                       flags=urdfFlags,
                       useFixedBase=False)

#enable collision between lower legs

for j in range(p.getNumJoints(quadruped)):
  print(p.getJointInfo(quadruped, j))

#2,5,8 and 11 are the lower legs
lower_legs = [2, 5, 8, 11]
for l0 in lower_legs:
  for l1 in lower_legs:
    if (l1 > l0):
      enableCollision = 1
      print("collision for pair", l0, l1,
            p.getJointInfo(quadruped, l0)[12],
            p.getJointInfo(quadruped, l1)[12], "enabled=", enableCollision)
      p.setCollisionFilterPair(quadruped, quadruped, 2, 5, enableCollision)

jointIds = []
paramIds = []
jointOffsets = []

  return log

#clid = p.connect(p.SHARED_MEMORY)
p.connect(p.GUI)
p.loadURDF("plane.urdf",[0,0,-0.3])
p.loadURDF("kuka_iiwa/model.urdf",[0,0,0])
p.loadURDF("cube.urdf",[2,2,5])
p.loadURDF("cube.urdf",[-2,-2,5])
p.loadURDF("cube.urdf",[2,-2,5])

log = readLogFile("LOG0001.txt")

recordNum = len(log)
itemNum = len(log[0])
print('record num:'),
print(recordNum)
print('item num:'),
print(itemNum)

for record in log:
    Id = record[2]
    pos = [record[3],record[4],record[5]]
    orn = [record[6],record[7],record[8],record[9]]
    p.resetBasePositionAndOrientation(Id,pos,orn)
    numJoints = p.getNumJoints(Id)
    for i in range (numJoints):
        jointInfo = p.getJointInfo(Id,i)
        qIndex = jointInfo[3]
        if qIndex > -1:
            p.resetJointState(Id,i,record[qIndex-7+17])
    sleep(0.0005)