Python pybullet.getJointState() Examples

def setRobot(self, robot, physicsClientId=None):
        self._robot = robot
        if physicsClientId != None:
            self._physicsClientId = physicsClientId

        joint_id_list = []
        joint_pos_list = []
        self._joint_number = 0
        for i in range(p.getNumJoints(robot)):
            jointInfo = p.getJointInfo(robot, i)
            if jointInfo[2] == 0:
                joint_id_list.append(jointInfo[0])
                joint_pos_list.append(p.getJointState(robot, jointInfo[0])[0])
                self._joint_number += 1
        print(self._joint_number)
        self._joint_id = np.array(joint_id_list, dtype=np.int32)
        self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
        self._joint_currentPos = np.array(joint_pos_list, dtype=np.float) 

def apply_action_fingers(self, action, obj_id=None):
        # move finger joints in position control
        assert len(action) == 2, ('finger joints are 2! The number of actions you passed is ', len(action))

        idx_fingers = [self._joint_name_to_ids['panda_finger_joint1'], self._joint_name_to_ids['panda_finger_joint2']]

        # use object id to check contact force and eventually stop the finger motion
        if obj_id is not None:
            _, forces = self.check_contact_fingertips(obj_id)
            # print("contact forces {}".format(forces))

            if forces[0] >= 20.0:
                action[0] = p.getJointState(self.robot_id, idx_fingers[0], physicsClientId=self._physics_client_id)[0]

            if forces[1] >= 20.0:
                action[1] = p.getJointState(self.robot_id, idx_fingers[1], physicsClientId=self._physics_client_id)[0]

        for i, idx in enumerate(idx_fingers):
            p.setJointMotorControl2(self.robot_id,
                                    idx,
                                    p.POSITION_CONTROL,
                                    targetPosition=action[i],
                                    force=10,
                                    maxVelocity=1,
                                    physicsClientId=self._physics_client_id) 

def _step(self, action):
    p.stepSimulation()
#    time.sleep(self.timeStep)
    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]
    theta, theta_dot, x, x_dot = self.state

    dv = 0.1
    deltav = [-10.*dv,-5.*dv, -2.*dv, -0.1*dv, 0, 0.1*dv, 2.*dv,5.*dv, 10.*dv][action]

    p.setJointMotorControl2(self.cartpole, 0, p.VELOCITY_CONTROL, targetVelocity=(deltav + self.state[3]))

    done =  x < -self.x_threshold \
                or x > self.x_threshold \
                or theta < -self.theta_threshold_radians \
                or theta > self.theta_threshold_radians
    reward = 1.0

    return np.array(self.state), reward, done, {} 

def _reset(self):
#    print("-----------reset simulation---------------")
    p.resetSimulation()
    self.cartpole = p.loadURDF(os.path.join(pybullet_data.getDataPath(),"cartpole.urdf"),[0,0,0])
    self.timeStep = 0.01
    p.setJointMotorControl2(self.cartpole, 1, p.VELOCITY_CONTROL, force=0)
    p.setGravity(0,0, -10)
    p.setTimeStep(self.timeStep)
    p.setRealTimeSimulation(0)

    initialCartPos = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
    initialAngle = self.np_random.uniform(low=-0.5, high=0.5, size=(1,))
    p.resetJointState(self.cartpole, 1, initialAngle)
    p.resetJointState(self.cartpole, 0, initialCartPos)

    self.state = p.getJointState(self.cartpole, 1)[0:2] + p.getJointState(self.cartpole, 0)[0:2]

    return np.array(self.state) 

def getMotorAngles(self):
    motorAngles = []
    for i in range(self.nMotors):
      jointState = p.getJointState(self.quadruped, self.motorIdList[i])
      motorAngles.append(jointState[0])
    motorAngles = np.multiply(motorAngles, self.motorDir)
    return motorAngles 

def step(self, action):
        yaw = 0

        while True:
            yaw += -0.75
            p.resetDebugVisualizerCamera(cameraDistance=1.2, cameraYaw=yaw, cameraPitch=-20, cameraTargetPosition=[0, 0, 1.0], physicsClientId=self.id)
            indices = [4, 5, 6]
            # indices = [14, 15, 16]
            deltas = [0.01, 0.01, -0.01]
            indices = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9]
            deltas = [0, 0, 0, 0, 0.01, 0.01, -0.01, 0, 0, 0]
            # indices = []
            # deltas = []
            # indices = np.array([0, 1, 2, 3, 4, 5, 6, 7, 8, 9]) + 10
            # deltas = [0, 0, 0, 0, -0.01, 0.01, -0.01, 0, 0, 0]
            for i, d in zip(indices, deltas):
                joint_position = p.getJointState(self.human, jointIndex=i, physicsClientId=self.id)[0]
                if joint_position + d > self.human_lower_limits[i] and joint_position + d < self.human_upper_limits[i]:
                    p.resetJointState(self.human, jointIndex=i, targetValue=joint_position+d, targetVelocity=0, physicsClientId=self.id)
            p.stepSimulation(physicsClientId=self.id)
            print('cameraYaw=%.2f, cameraPitch=%.2f, distance=%.2f' % p.getDebugVisualizerCamera(physicsClientId=self.id)[-4:-1])
            self.enforce_realistic_human_joint_limits()
            time.sleep(0.05)

        return [], None, None, None 

def GetMotorAngles(self):
        """Get the eight motor angles at the current moment.

        Returns:
          Motor angles.
        """
        motor_angles = [
            p.getJointState(self.minitaur, motor_id)[0]
            for motor_id in self._motor_id_list
        ]
        motor_angles = np.multiply(motor_angles, self.motor_direction)
        return motor_angles 

def _getArmPosition(self):
        '''
        Get arm information.

        Returns:
        ---------
        q: vector of joint positions
        dq: vector of joint velocities
        '''
        q = [0.] * 6
        dq = [0.] * 6
        for i in xrange(6):
            q[i], dq[i] = pb.getJointState(self.handle, i)[:2]
        return np.array(q), np.array(dq) 

def GetMotorVelocities(self):
        """Get the velocity of all eight motors.

        Returns:
          Velocities of all eight motors.
        """
        motor_velocities = [
            p.getJointState(self.minitaur, motor_id)[1]
            for motor_id in self._motor_id_list
        ]
        motor_velocities = np.multiply(motor_velocities, self.motor_direction)
        return motor_velocities 

def GetMotorTorques(self):
        """Get the amount of torques the motors are exerting.

        Returns:
          Motor torques of all eight motors.
        """
        if self.accurate_motor_model_enabled or self.pd_control_enabled:
            return self.observed_motor_torques
        else:
            motor_torques = [
                p.getJointState(self.minitaur, motor_id)[3] for motor_id in self._motor_id_list ]
            motor_torques = np.multiply(motor_torques, self.motor_direction)
        return motor_torques 

def get_state(self):
        """Get state of joint
           Position is defined in real world scale """
        x, vx,_,_ = p.getJointState(self.bodies[self.bodyIndex],self.jointIndex)
        if self.jointType == p.JOINT_PRISMATIC:
            x  *= self.scale
            vx *= self.scale
        return x, vx 

def get_state(self):
		x, vx,_,_ = p.getJointState(self.bodies[self.bodyIndex],self.jointIndex)
		return x, vx 

def get_state(self):
        x, vx,_,_ = p.getJointState(self.bodies[self.bodyIndex],self.jointIndex)
        return x, vx 

def __init__(self, robot, physicsClientId, timeStep, kp, kd, torque, max_velocity):
        self._robot = robot
        self._physicsClientId = physicsClientId
        jointNameToId = {}
        joint_id_list = []
        joint_pos_list = []
        self._joint_number = 0
        for i in range(p.getNumJoints(self._robot, physicsClientId=self._physicsClientId)):
            jointInfo = p.getJointInfo(self._robot, i, physicsClientId=self._physicsClientId)
            if jointInfo[2] == 0:
                joint_id_list.append(jointInfo[0])
                joint_pos_list.append(p.getJointState(self._robot, jointInfo[0], physicsClientId=self._physicsClientId)[0])
                jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
                self._joint_number += 1
        self._joint_id = np.array(joint_id_list, dtype=np.int32)
        self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
        self._joint_currentPos = np.array(joint_pos_list, dtype=np.float)

        self._jointNameToId = jointNameToId
        self._kp = kp
        self._kd = kd
        self._torque = torque
        self._max_velocity = max_velocity
        self._timeStep = timeStep
        # print(self._joint_id)
        # print(self._joint_targetPos)
        # print(self._jointNameToId) 

def setRobot(self, robot, physicsClientId=None):
        self._robot = robot
        if physicsClientId != None:
            self._physicsClientId = physicsClientId
        jointNameToId = {}
        joint_id_list = []
        joint_pos_list = []
        self._joint_number = 0
        for i in range(p.getNumJoints(self._robot, physicsClientId=self._physicsClientId)):
            jointInfo = p.getJointInfo(self._robot, i, physicsClientId=self._physicsClientId)
            if jointInfo[2] == 0:
                joint_id_list.append(jointInfo[0])
                joint_pos_list.append(p.getJointState(self._robot, jointInfo[0], physicsClientId=self._physicsClientId)[0])
                jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
                self._joint_number += 1
        self._joint_id = np.array(joint_id_list, dtype=np.int32)
        self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
        self._joint_currentPos = np.array(joint_pos_list, dtype=np.float) 

def getMotorAngle(self):
        for i in range(self._joint_number):
            self._joint_currentPos[i] = p.getJointState(self._robot, self._joint_id[i], physicsClientId=self._physicsClientId)[0]
        return self._joint_currentPos 

def __init__(self, robot, physicsClientId, timeStep, kp, kd, torque, max_velocity):
        self._robot = robot
        self._physicsClientId = physicsClientId
        jointNameToId = {}
        joint_id_list = []
        joint_pos_list = []
        self._joint_number = 0
        for i in range(p.getNumJoints(self._robot, physicsClientId=self._physicsClientId)):
            jointInfo = p.getJointInfo(self._robot, i, physicsClientId=self._physicsClientId)
            if jointInfo[2] == 0:
                joint_id_list.append(jointInfo[0])
                joint_pos_list.append(p.getJointState(
                    self._robot, jointInfo[0], physicsClientId=self._physicsClientId)[0])
                jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
                self._joint_number += 1
        self._joint_id = np.array(joint_id_list, dtype=np.int32)
        self._joint_targetPos = np.array(joint_pos_list, dtype=np.float)
        self._joint_currentPos = np.array(joint_pos_list, dtype=np.float)

        self._jointNameToId = jointNameToId
        self._kp = kp
        self._kd = kd
        self._torque = torque
        self._max_velocity = max_velocity
        self._timeStep = timeStep
        # print(self._joint_id)
        # print(self._joint_targetPos)
        # print(self._jointNameToId) 

def getMotorAngle(self):
        for i in range(self._joint_number):
            self._joint_currentPos[i] = p.getJointState(
                self._robot, self._joint_id[i], physicsClientId=self._physicsClientId)[0]
        return self._joint_currentPos 

def get_state(self):
        """Get state of joint
           Position is defined in real world scale """
        x, vx,_,_ = p.getJointState(self.bodies[self.bodyIndex],self.jointIndex)
        if self.jointType == p.JOINT_PRISMATIC:
            x  *= self.scale
            vx *= self.scale
        return x, vx 

def _getGripper(self):
        return pb.getJointState(self.handle, self.left_finger) 

def _getArmPosition(self):
        '''
        Get arm information.

        Returns:
        ---------
        q: vector of joint positions
        dq: vector of joint velocities
        '''
        q = [0.] * 6
        dq = [0.] * 6
        for i in xrange(6):
            q[i], dq[i] = pb.getJointState(self.handle, i)[:2]
        return np.array(q), np.array(dq) 

def _getGripper(self):
        return pb.getJointState(self.handle, self.left_finger) 

def _getArmPosition(self):
        q = [0.] * 6
        for i in xrange(6):
            q = pb.getJointState(self.handle, i) 

def set_joint_vel(body, joint, vel):
        _, vel, _, _ = p.getJointState(body, joint)
        p.resetJointState(body, joint, pos, list(vel)) 

def get_joint_torque(body, joint):
        _, _, _, torque = p.getJointState(body, joint)
        return np.array(torque, dtype=np.float32) 

def get_joint_force(body, joint):
        # TODO: Definition of react_force?
        _, _, react_force, _ = p.getJointState(body, joint)
        return np.array(react_force, dtype=np.float32) 

def get_joint_vel(body, joint):
        _, vel, react_force, torque = p.getJointState(body, joint)
        return np.array(vel, dtype=np.float32) 

def get_joint_pos(body, joint):
        pos, _, react_force, torque = p.getJointState(body, joint)
        return np.array(pos, dtype=np.float32) 

def setJoints(self, joints):
        """Set joint targets for motor control in simulation
        
        Arguments:
            joints {dict} -- dict of joint name -> angle (float, radian)
        
        Raises:
            Exception: if a joint is not found, exception is raised

        Returns:
            applied {dict} -- dict of joint states (position, velocity, reaction forces, applied torque)
        """
        applied = {}

        for name in joints.keys():
            if name in self.joints:
                if name.endswith('_speed'):
                    p.setJointMotorControl2(
                    self.robot, self.joints[name], p.VELOCITY_CONTROL, targetVelocity=joints[name])
                else:
                    if name in self.maxTorques:
                        maxTorque = self.maxTorques[name]
                        p.setJointMotorControl2(
                            self.robot, self.joints[name], p.POSITION_CONTROL, joints[name], force=maxTorque)
                    else:
                        p.setJointMotorControl2(
                            self.robot, self.joints[name], p.POSITION_CONTROL, joints[name])

                applied[name] = p.getJointState(self.robot, self.joints[name])
            else:
                raise Exception("Can't find joint %s" % name)

        return applied 

def setup_inverse_kinematics(self, urdf_path):
        """
        This function is responsible for doing any setup for inverse kinematics.
        Inverse Kinematics maps end effector (EEF) poses to joint angles that
        are necessary to achieve those poses. 
        """

        # These indices come from the urdf file we're using
        self.effector_right = 27
        self.effector_left = 45

        # Use PyBullet to handle inverse kinematics.
        # Set up a connection to the PyBullet simulator.
        p.connect(p.DIRECT)
        p.resetSimulation()

        self.ik_robot = p.loadURDF(urdf_path, (0, 0, 0), useFixedBase=1)

        # Relevant joints we care about. Many of the joints are fixed and don't count, so
        # we need this second map to use the right ones.
        self.actual = [13, 14, 15, 16, 17, 19, 20, 31, 32, 33, 34, 35, 37, 38]

        self.num_joints = p.getNumJoints(self.ik_robot)
        n = p.getNumJoints(self.ik_robot)
        self.rest = []
        self.lower = []
        self.upper = []
        self.ranges = []
        for i in range(n):
            info = p.getJointInfo(self.ik_robot, i)
            # Retrieve lower and upper ranges for each relevant joint
            if info[3] > -1:
                self.rest.append(p.getJointState(self.ik_robot, i)[0])
                self.lower.append(info[8])
                self.upper.append(info[9])
                self.ranges.append(info[9] - info[8])

        # Simulation will update as fast as it can in real time, instead of waiting for
        # step commands like in the non-realtime case.
        p.setRealTimeSimulation(1)