Esempio n. 1
0
 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)
Esempio n. 2
0
	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)
Esempio n. 5
0
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)
Esempio n. 7
0
 def get_joint_state_multi_dof(self, *args, **kwargs):
     return pb.getJointStateMultiDof(*args,
                                     **kwargs,
                                     physicsClientId=self.bullet_cid)
Esempio n. 8
0
        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()
Esempio n. 9
0
#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)
Esempio n. 10
0
    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)