def repos(teoId, com_0 = np.array([0,0,l9+l10+l11+l12-0.001]), orientation=np.array([0,0,0])):
p.resetBasePositionAndOrientation(teoId, com_0+np.array([8.67, -0.03, 24.06])/1000, p.getQuaternionFromEuler(orientation))
n_joints = p.getNumJoints(teoId)
name2id={}
for i in range(n_joints):
name2id[p.getJointInfo(teoId,i)[1]]=i
n_joints = p.getNumJoints(teoId)
jointIndices = [0] * n_joints
targetPos = [0.0] * n_joints
maxVelocities = [radians(1)] * n_joints
maxForces = [0.0] * n_joints
for i in range(n_joints):
jointIndices[i] = i
maxForces[i] = p.getJointInfo(teoId,i)[10]
targetPos[i] = 0
rightLegAngles= ik.rl_com_from_foot(com_0 , orientation)
leftLegAngles = ik.ll_com_from_foot(com_0 , orientation)
for i, index in enumerate(rightLegIndices):
if (not np.isnan(rightLegAngles[i])) and (rightLegAngles[i] > p.getJointInfo(teoId, index)[8] and rightLegAngles[i] < p.getJointInfo(teoId, index)[9]):
targetPos[index]=rightLegAngles[i]
for i, index in enumerate(leftLegIndices):
if (not np.isnan(leftLegAngles[i])) and ( leftLegAngles[i] > p.getJointInfo(teoId, index)[8] and leftLegAngles[i] < p.getJointInfo(teoId, index)[9]):
targetPos[index]=leftLegAngles[i]
for jointIndex in range(n_joints):
p.resetJointState(teoId,
jointIndex,
targetPos[jointIndex])
mode = p.POSITION_CONTROL
p.setJointMotorControlArray(teoId,
jointIndices,
controlMode=mode,
forces=maxForces,
targetPositions = targetPos
)
def reset_sim(com_0 = np.array([0,0,l9+l10+l11+l12-0.001]), orientation=np.array([0,0,0]), g=-9.79983):
p.resetSimulation()
p.setTimeStep(timestep)
p.setGravity(0,0,g)
planeId = p.loadURDF("plane.urdf")
teoStartOrientation = p.getQuaternionFromEuler(-orientation)
teoStartPosition = [0,0,com_0[2]]
urdf_path = os.path.join(urdf_root,"TEO_wobble.urdf")
teoId = p.loadURDF(urdf_path,teoStartPosition ,teoStartOrientation)
#print("TEO pos =",p.getBasePositionAndOrientation(teoId)[0])
n_joints = p.getNumJoints(teoId)
name2id={}
for i in range(n_joints):
name2id[p.getJointInfo(teoId,i)[1]]=i
n_joints = p.getNumJoints(teoId)
jointIndices = [0] * n_joints
targetPos = [0.0] * n_joints
maxVelocities = [radians(1)] * n_joints
maxForces = [0.0] * n_joints
for i in range(n_joints):
jointIndices[i] = i
maxForces[i] = p.getJointInfo(teoId,i)[10]
targetPos[i] = 0
rightLegAngles= ik.rl_com_from_foot(com_0 , orientation)
leftLegAngles = ik.ll_com_from_foot(com_0 , orientation)
'''
print("com =",com_0)
print("com_f =",com_0+np.array([0,l16,-l12]))
print()
print("Hst0_r =",[0,-l16,l9+l10+l11+l12])
print("rightLegP(com_0) =",rightLegP(com_0))
print("rightLegAngles =",[degrees(q) for q in rightLegAngles])
print()
print("Hst0_l =",[0,l16,l9+l10+l11+l12])
print("leftLegP(com_0) =",leftLegP(com_0))
print("leftLegAngles =",[degrees(q) for q in leftLegAngles])
'''
for i, index in enumerate(rightLegIndices):
if (not np.isnan(rightLegAngles[i])) and (rightLegAngles[i] > p.getJointInfo(teoId, index)[8] and rightLegAngles[i] < p.getJointInfo(teoId, index)[9]):
targetPos[index]=rightLegAngles[i]
for i, index in enumerate(leftLegIndices):
if (not np.isnan(leftLegAngles[i])) and ( leftLegAngles[i] > p.getJointInfo(teoId, index)[8] and leftLegAngles[i] < p.getJointInfo(teoId, index)[9]):
targetPos[index]=leftLegAngles[i]
for jointIndex in range(n_joints):
p.resetJointState(teoId,
jointIndex,
targetPos[jointIndex])
mode = p.POSITION_CONTROL
p.setJointMotorControlArray(teoId,
jointIndices,
controlMode=mode,
forces=maxForces,
targetPositions = targetPos
)
return teoId
targetPos = [0.0] * n_joints
maxVelocities = [radians(1)] * n_joints
maxForces = [0.0] * n_joints
for i in
range(n_joints):
jointIndices[i] = i
maxForces[i] = p.getJointInfo(teoId,i)[10]
targetPos[i] = 0
rightLegAngles= ik.rl_com_from_foot(com_0 , orientation)
leftLegAngles = ik.ll_com_from_foot(com_0 , orientation)
for i, index in enumerate(rightLegIndices):
if (not np.isnan(rightLegAngles[i])) and (rightLegAngles[i] > p.getJointInfo(teoId, index)[8] and rightLegAngles[i] < p.getJointInfo(teoId, index)[9]):
targetPos[index]=rightLegAngles[i]
for i, index in enumerate(leftLegIndices):
if (not np.isnan(leftLegAngles[i])) and ( leftLegAngles[i] > p.getJointInfo(teoId, index)[8] and leftLegAngles[i] < p.getJointInfo(teoId, index)[9]):
targetPos[index]=leftLegAngles[i]
for jointIndex in range(n_joints):
p.resetJointState(teoId,
jointIndex,
targetPos[jointIndex])
mode = p.POSITION_CONTROL