def loosenModel(self):
nJoints = p.getNumJoints(self.cliffordID,
physicsClientId=self.physicsClientId)
tireIndices = [
self.linkNameToID[name]
for name in ['frtire', 'fltire', 'brtire', 'bltire']
]
for i in range(nJoints):
if len(p.getJointStateMultiDof(bodyUniqueId=self.cliffordID,jointIndex=i,physicsClientId=self.physicsClientId)[0]) == 4 \
and not self.params["fixedSuspension"]:
p.setJointMotorControlMultiDof(
bodyUniqueId=self.cliffordID,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=[0, 0, 0, 1],
positionGain=0,
velocityGain=0,
force=[0, 0, 0],
physicsClientId=self.physicsClientId)
dynamicsData = p.getDynamicsInfo(
self.cliffordID, i, physicsClientId=self.physicsClientId)
massScale = self.params["massScale"]
if i in tireIndices:
massScale = massScale * self.params['tireMassScale']
newMass = dynamicsData[0] * massScale
newInertia = [
dynamicsData[2][j] * massScale
for j in range(len(dynamicsData[2]))
]
p.changeDynamics(self.cliffordID,
i,
mass=newMass,
localInertiaDiagonal=newInertia,
linearDamping=0.2,
angularDamping=0.2,
restitution=0,
physicsClientId=self.physicsClientId)
springJointNames = [
'brslower2upper', 'blslower2upper', 'frslower2upper',
'flslower2upper'
]
springConstant = 0
springDamping = 0
maxSpringForce = 0
for name in springJointNames:
#JointInfo = p.getJointInfo(self.cliffordID,self.jointNameToID[name],physicsClientId=self.physicsClientId)
#p.setJointMotorControlArray(bodyUniqueId=self.cliffordID,
# jointIndices=[self.jointNameToID[name]],
# controlMode=p.POSITION_CONTROL,
# targetPositions=[0],
# positionGains=[springConstant],
# velocityGains=[springDamping],
# forces=[maxSpringForce],physicsClientId=self.physicsClientId)
if not self.params["fixedSuspension"]:
p.changeDynamics(self.cliffordID,
self.jointNameToID[name],
jointLowerLimit=self.params["susLowerLimit"],
jointUpperLimit=self.params["susUpperLimit"],
physicsClientId=self.physicsClientId)
def __get_joint_positions_as_list(self): joint_position_list = [] for i in range(p.getNumJoints(self.body_id)): joint_info = p.getJointInfo(self.body_id, i) if joint_info[2] == p.JOINT_SPHERICAL: joint_state = p.getJointStateMultiDof(self.body_id, i) elif joint_info[2] == p.JOINT_REVOLUTE: joint_state = p.getJointState(self.body_id, i) joint_position_list.append(joint_state[0]) return joint_position_list
def set_pose(robot, pose):
num_joints = pybullet.getNumJoints(robot)
root_pos = get_root_pos(pose)
root_rot = get_root_rot(pose)
pybullet.resetBasePositionAndOrientation(robot, root_pos, root_rot)
for j in range(num_joints):
j_info = pybullet.getJointInfo(robot, j)
j_state = pybullet.getJointStateMultiDof(robot, j)
j_pose_idx = j_info[3]
j_pose_size = len(j_state[0])
j_vel_size = len(j_state[1])
if (j_pose_size > 0):
j_pose = pose[j_pose_idx:(j_pose_idx + j_pose_size)]
j_vel = np.zeros(j_vel_size)
pybullet.resetJointStateMultiDof(robot, j, j_pose, j_vel)
return
def loosenModel(self):
nJoints = p.getNumJoints(self.cliffordID,
physicsClientId=self.physicsClientId)
for i in range(nJoints):
if len(
p.getJointStateMultiDof(
bodyUniqueId=self.cliffordID,
jointIndex=i,
physicsClientId=self.physicsClientId)[0]) == 4:
p.setJointMotorControlMultiDof(
bodyUniqueId=self.cliffordID,
jointIndex=i,
controlMode=p.POSITION_CONTROL,
targetPosition=[0, 0, 0, 1],
positionGain=0,
velocityGain=0,
force=[0, 0, 0],
physicsClientId=self.physicsClientId)
springJointNames = [
'brslower2upper', 'blslower2upper', 'frslower2upper',
'flslower2upper'
]
springConstant = 0
springDamping = 0
maxSpringForce = 0
for name in springJointNames:
JointInfo = p.getJointInfo(self.cliffordID,
self.jointNameToID[name],
physicsClientId=self.physicsClientId)
p.setJointMotorControlArray(
bodyUniqueId=self.cliffordID,
jointIndices=[self.jointNameToID[name]],
controlMode=p.POSITION_CONTROL,
targetPositions=[0],
positionGains=[springConstant],
velocityGains=[springDamping],
forces=[maxSpringForce],
physicsClientId=self.physicsClientId)
from Humanoid_Basic_Env.resources.tiny.tinyAgent import TinyAgent
# Debug tests
clientID = p.connect(p.GUI)
p.setRealTimeSimulation(False)
test = TinyAgent(clientID)
# testing joint control
p.enableJointForceTorqueSensor(
bodyUniqueId=test.get_ids(),
jointIndex=1,
enableSensor=True
)
for i in range(3):
actions = 0.2*np.random.random(size=(3,)) - 0.1
time.sleep(0.02)
for x in range(3):
test.applyActions(actions)
time.sleep(0.2)
print(
p.getJointStateMultiDof(
bodyUniqueId=test.get_ids(),
jointIndex=1,
)[2:]
)
p.disconnect()
p.setJointMotorControlMultiDof(humanoid2,
leftElbow,
p.POSITION_CONTROL,
targetPosition=leftElbowRot,
positionGain=kp,
force=[maxForce])
kinematicHumanoid4 = True
if (kinematicHumanoid4):
p.resetJointStateMultiDof(humanoid4, chest, chestRot)
p.resetJointStateMultiDof(humanoid4, neck, neckRot)
p.resetJointStateMultiDof(humanoid4, rightHip, rightHipRot)
p.resetJointStateMultiDof(humanoid4, rightKnee, rightKneeRot)
p.resetJointStateMultiDof(humanoid4, rightAnkle, rightAnkleRot)
p.resetJointStateMultiDof(humanoid4, rightShoulder, rightShoulderRot)
p.resetJointStateMultiDof(humanoid4, rightElbow, rightElbowRot)
p.resetJointStateMultiDof(humanoid4, leftHip, leftHipRot)
p.resetJointStateMultiDof(humanoid4, leftKnee, leftKneeRot)
p.resetJointStateMultiDof(humanoid4, leftAnkle, leftAnkleRot)
p.resetJointStateMultiDof(humanoid4, leftShoulder, leftShoulderRot)
p.resetJointStateMultiDof(humanoid4, leftElbow, leftElbowRot)
p.stepSimulation()
if showJointMotorTorques:
for j in range(p.getNumJoints(humanoid2)):
jointState = p.getJointStateMultiDof(humanoid2, j)
print("jointStateMultiDof[", j, "].pos=", jointState[0])
print("jointStateMultiDof[", j, "].vel=", jointState[1])
print("jointStateMultiDof[", j, "].jointForces=", jointState[3])
time.sleep(timeStep)
def get_joint_state_multi_dof(self, *args, **kwargs):
return pb.getJointStateMultiDof(*args,
**kwargs,
physicsClientId=self.bullet_cid)
LeftHandIndex1, LeftHandIndex2, LeftHandIndex3, LeftHandMiddle1,
LeftHandMiddle2, LeftHandMiddle3, LeftHandPinky1, LeftHandPinky2,
LeftHandPinky3, LeftHandRing1, LeftHandRing2, LeftHandRing3,
LeftHandThumb2, LeftHandThumb3, RightHandIndex1, RightHandIndex2,
RightHandIndex3, RightHandMiddle1, RightHandMiddle2, RightHandMiddle3,
RightHandPinky1, RightHandPinky2, RightHandPinky3, RightHandRing1,
RightHandRing2, RightHandRing3, RightHandThumb2, RightHandThumb3
]
forces = np.empty(len(motors))
forces.fill(force)
print("clench force=", force)
# p.setJointMotorControlArray(robot, motors, controlMode=p.TORQUE_CONTROL, forces=forces)
print("run simulation")
# step through the simluation
for i in range(10000):
p.stepSimulation()
time.sleep(1. / 240.)
# clench_fists()
# (12, b'link_LeftArm', 2, 29, 24, 0, 0.0, 0.0, -2.0, 2.0, 1000.0, 0.5, b'link_LeftArm', (0.0, 0.0, 0.0), (0.1055, 0.02299, -0.00565), (0.0, 0.0, 0.0, 1.0), 11)
# print(p.getJointInfo(robot, LeftArm))
for j in range(p.getNumJoints(robot)):
jointState = p.getJointStateMultiDof(robot, j)
print("jointStateMultiDof[", j, "].pos=", jointState[0])
print("jointStateMultiDof[", j, "].vel=", jointState[1])
print("jointStateMultiDof[", j, "].jointForces=", jointState[3])
p.disconnect()
#choose connection method: GUI, DIRECT, SHARED_MEMORY
p.connect(p.GUI)
p.loadURDF(os.path.join(pybullet_data.getDataPath(), "plane.urdf"), 0, 0, 0)
#load URDF, given a relative or absolute file+path
obj = p.loadMJCF(os.path.join(pybullet_data.getDataPath(), "mjcf/model.xml"))
posX = 0
posY = 3
posZ = 2
#query the number of joints of the object
print(obj[0])
numJoints = p.getNumJoints(obj[0])
for i in range(numJoints):
print(p.getJointInfo(obj[0], i))
print(p.getJointStateMultiDof(obj[0], i))
print()
print("Number of Joints:", numJoints)
#set the gravity acceleration
p.setGravity(0, 0, -9.8)
#p.setRealTimeSimulation(True)
f = 0
start = time.time()
while True: #time.time() < t_end:
p.stepSimulation()
current = time.time()
if f % 30 == 0:
print(current - start)
time.sleep(0.02)
force_leg_move =PdController.computePD(botId,leg_move_ID,fsm.states[state_now].targetAngles[leg_move_ID-2],0,300,30,370)
#print("force_leg_move:{}".format(force_leg_move))
force_leg_stand=-force_torso-force_leg_move
#print("force_leg_stand:{}".format(force_leg_stand))
#print("torso_pos:{}".format(p.getJointState(botId,2)[0]))
#print("joint_pos:{}".format(p.getJointState(botId,3)[0]))
p.setJointMotorControl2(botId, leg_move_ID, p.TORQUE_CONTROL,force=force_leg_move)
p.setJointMotorControl2(botId, leg_stand_ID, p.TORQUE_CONTROL,force=force_leg_stand)
jointID=[4,5,7,8]
for i in jointID:
if(i==8 or i==5):
force=PdController.computePD(botId, i, fsm.states[state_now].targetAngles[i-2], 0, 20, 2, 370)
else:
force = PdController.computePD(botId, i, fsm.states[state_now].targetAngles[i - 2], 0, 300, 30, 370)
p.setJointMotorControl2(botId, i, p.TORQUE_CONTROL,force=force)
print(p.getJointStateMultiDof(botId, 8)[2])
force=p.readUserDebugParameter(push)
keys = p.getKeyboardEvents()
qKey = ord('1')
if qKey in keys and keys[qKey] & p.KEY_WAS_TRIGGERED:
push_flag=True
if(push_flag==True):
print("push")
p.applyExternalForce(botId,2,[0,force,0],[0,0,0],flags=p.LINK_FRAME)
timer += step
if(timer>=0.1):
timer=0
push_flag=False
time.sleep(1/240)
time.sleep(1000)
p.setJointMotorControlMultiDof(humanoid2,
leftElbow,
p.POSITION_CONTROL,
targetPosition=leftElbowRot,
positionGain=kp,
force=[maxForce])
kinematicHumanoid4 = True
if (kinematicHumanoid4):
p.resetJointStateMultiDof(humanoid4, chest, chestRot)
p.resetJointStateMultiDof(humanoid4, neck, neckRot)
p.resetJointStateMultiDof(humanoid4, rightHip, rightHipRot)
p.resetJointStateMultiDof(humanoid4, rightKnee, rightKneeRot)
p.resetJointStateMultiDof(humanoid4, rightAnkle, rightAnkleRot)
p.resetJointStateMultiDof(humanoid4, rightShoulder, rightShoulderRot)
p.resetJointStateMultiDof(humanoid4, rightElbow, rightElbowRot)
p.resetJointStateMultiDof(humanoid4, leftHip, leftHipRot)
p.resetJointStateMultiDof(humanoid4, leftKnee, leftKneeRot)
p.resetJointStateMultiDof(humanoid4, leftAnkle, leftAnkleRot)
p.resetJointStateMultiDof(humanoid4, leftShoulder, leftShoulderRot)
p.resetJointStateMultiDof(humanoid4, leftElbow, leftElbowRot)
p.stepSimulation()
if showJointMotorTorques:
for j in range(p.getNumJoints(humanoid2)):
jointState = p.getJointStateMultiDof(humanoid2, j)
print("jointStateMultiDof[", j, "].pos=", jointState[0])
print("jointStateMultiDof[", j, "].vel=", jointState[1])
print("jointStateMultiDof[", j, "].jointForces=", jointState[3])
time.sleep(timeStep)
