How to use the pybullet.resetJointState function in pybullet

To help you get started, we’ve selected a few pybullet examples, based on popular ways it is used in public projects.

Secure your code as it's written. Use Snyk Code to scan source code in minutes - no build needed - and fix issues immediately.

github StanfordVL / GibsonEnvV2 / gibson2 / core / physics / robot_bases.py View on Github external
def set_state(self, x, vx):
        """Set state of joint
           x is defined in real world scale """
        if self.joint_type == p.JOINT_PRISMATIC:
            x /= self.scale
            vx /= self.scale
        p.resetJointState(self.bodies[self.body_index], self.joint_index, x, vx)
github Healthcare-Robotics / assistive-gym / assistive_gym / envs / agents / agent.py View on Github external
def enforce_joint_limits(self, indices=None):
        if indices is None:
            indices = self.all_joint_indices
        joint_angles = self.get_joint_angles_dict(indices)
        if self.lower_limits is None or len(indices) > len(self.lower_limits):
            self.update_joint_limits()
        for j in indices:
            if joint_angles[j] < self.lower_limits[j]:
                p.resetJointState(self.body, jointIndex=j, targetValue=self.lower_limits[j], targetVelocity=0, physicsClientId=self.id)
            elif joint_angles[j] > self.upper_limits[j]:
                p.resetJointState(self.body, jointIndex=j, targetValue=self.upper_limits[j], targetVelocity=0, physicsClientId=self.id)
github PacktPublishing / Deep-Reinforcement-Learning-Hands-On / ch15 / pybulletgym / envs / robots / robot_bases.py View on Github external
def set_state(self, x, vx):
		p.resetJointState(self.bodies[self.bodyIndex], self.jointIndex, x, vx)
github Healthcare-Robotics / assistive-gym / assistive_gym / envs / env.py View on Github external
if 3 in self.human_controllable_joint_indices:
            # Right human arm
            tz, tx, ty, qe = [j[0] for j in p.getJointStates(self.human, jointIndices=[3, 4, 5, 6], physicsClientId=self.id)]
            # Transform joint angles to match those from the Matlab data
            tz2 = (-tz + 2*np.pi) % (2*np.pi)
            tx2 = (tx + 2*np.pi) % (2*np.pi)
            ty2 = -ty
            qe2 = (-qe + 2*np.pi) % (2*np.pi)
            result = self.human_limits_model.predict_classes(np.array([[tz2, tx2, ty2, qe2]]))
            if result == 1:
                # This is a valid pose for the person
                self.right_arm_previous_valid_pose = [tz, tx, ty, qe]
            elif result == 0 and self.right_arm_previous_valid_pose is not None:
                # The person is in an invalid pose. Move them back to the most recent valid pose.
                for i, j in enumerate([3, 4, 5, 6]):
                    p.resetJointState(self.human, jointIndex=j, targetValue=self.right_arm_previous_valid_pose[i], targetVelocity=0, physicsClientId=self.id)
        if 13 in self.human_controllable_joint_indices:
            # Left human arm
            tz, tx, ty, qe = [j[0] for j in p.getJointStates(self.human, jointIndices=[13, 14, 15, 16], physicsClientId=self.id)]
            # Transform joint angles to match those from the Matlab data
            tz2 = (tz + 2*np.pi) % (2*np.pi)
            tx2 = (tx + 2*np.pi) % (2*np.pi)
            ty2 = ty
            qe2 = (-qe + 2*np.pi) % (2*np.pi)
            result = self.human_limits_model.predict_classes(np.array([[tz2, tx2, ty2, qe2]]))
            if result == 1:
                # This is a valid pose for the person
                self.left_arm_previous_valid_pose = [tz, tx, ty, qe]
            elif result == 0 and self.left_arm_previous_valid_pose is not None:
                # The person is in an invalid pose. Move them back to the most recent valid pose.
                for i, j in enumerate([13, 14, 15, 16]):
                    p.resetJointState(self.human, jointIndex=j, targetValue=self.left_arm_previous_valid_pose[i], targetVelocity=0, physicsClientId=self.id)
github StanfordVL / GibsonEnvV2 / gibson2 / core / physics / robot_bases.py View on Github external
def set_state(self, x, vx):
        """Set state of joint
           x is defined in real world scale """
        if self.joint_type == p.JOINT_PRISMATIC:
            x /= self.scale
            vx /= self.scale
        p.resetJointState(self.bodies[self.body_index], self.joint_index, x, vx)
github Froser / gamemachine / coding / 3rdparty / bullet3 / examples / pybullet / quadruped.py View on Github external
#right back leg
p.resetJointState(quadruped,12,1.57)
p.resetJointState(quadruped,14,-2.2)
p.resetJointState(quadruped,15,-1.57)
p.resetJointState(quadruped,17,2.2)
p.createConstraint(quadruped,14,quadruped,17,p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])

p.setJointMotorControl(quadruped,12,p.POSITION_CONTROL,1.57,1)
p.setJointMotorControl(quadruped,13,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,14,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,15,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,16,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,17,p.VELOCITY_CONTROL,0,0)

#left back leg
p.resetJointState(quadruped,18,1.57)
p.resetJointState(quadruped,20,-2.2)
p.resetJointState(quadruped,21,-1.57)
p.resetJointState(quadruped,23,2.2)
p.createConstraint(quadruped,20,quadruped,23,p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])

p.setJointMotorControl(quadruped,18,p.POSITION_CONTROL,1.57,1)
p.setJointMotorControl(quadruped,19,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,20,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,21,p.POSITION_CONTROL,-1.57,1)
p.setJointMotorControl(quadruped,22,p.VELOCITY_CONTROL,0,0)
p.setJointMotorControl(quadruped,23,p.VELOCITY_CONTROL,0,0)



p_gain = 2
speed = 10
github StanfordVL / GibsonEnvV2 / gibson2 / core / physics / drivers / minitaur.py View on Github external
def _ResetPoseForLeg(self, leg_id, add_constraint):
        """Reset the initial pose for the leg.

        Args:
          leg_id: It should be 0, 1, 2, or 3, which represents the leg at
            front_left, back_left, front_right and back_right.
          add_constraint: Whether to add a constraint at the joints of two feet.
        """
        knee_friction_force = 0
        half_pi = math.pi / 2.0
        knee_angle = -2.1834

        leg_position = self.LEG_POSITION[leg_id]
        p.resetJointState(self.minitaur,
                          self.joint_name_to_id["motor_" + leg_position + "L_joint"],
                          self.motor_direction[2 * leg_id] * half_pi,
                          targetVelocity=0)
        p.resetJointState(self.minitaur,
                          self.joint_name_to_id["knee_" + leg_position + "L_link"],
                          self.motor_direction[2 * leg_id] * knee_angle,
                          targetVelocity=0)
        p.resetJointState(self.minitaur,
                          self.joint_name_to_id["motor_" + leg_position + "R_joint"],
                          self.motor_direction[2 * leg_id + 1] * half_pi,
                          targetVelocity=0)
        p.resetJointState(self.minitaur,
                          self.joint_name_to_id["knee_" + leg_position + "R_link"],
                          self.motor_direction[2 * leg_id + 1] * knee_angle,
                          targetVelocity=0)
        if add_constraint:
github erwincoumans / pybullet_robots / corl_demo / minitaur.py View on Github external
knee_friction_force = 0
    half_pi = math.pi / 2.0
    knee_angle = -2.1834

    leg_position = LEG_POSITION[leg_id]
    pybullet.resetJointState(
        self.quadruped,
        self._joint_name_to_id["motor_" + leg_position + "L_joint"],
        self._motor_direction[2 * leg_id] * half_pi,
        targetVelocity=0)
    pybullet.resetJointState(
        self.quadruped,
        self._joint_name_to_id["knee_" + leg_position + "L_link"],
        self._motor_direction[2 * leg_id] * knee_angle,
        targetVelocity=0)
    pybullet.resetJointState(
        self.quadruped,
        self._joint_name_to_id["motor_" + leg_position + "R_joint"],
        self._motor_direction[2 * leg_id + 1] * half_pi,
        targetVelocity=0)
    pybullet.resetJointState(
        self.quadruped,
        self._joint_name_to_id["knee_" + leg_position + "R_link"],
        self._motor_direction[2 * leg_id + 1] * knee_angle,
        targetVelocity=0)
    if add_constraint:
      pybullet.createConstraint(
          self.quadruped, self._joint_name_to_id["knee_"
                                                 + leg_position + "R_link"],
          self.quadruped, self._joint_name_to_id["knee_"
                                                 + leg_position + "L_link"],
          pybullet.JOINT_POINT2POINT, [0, 0, 0],
github bhyang / replab / replab_rl / gym_replab / gym_replab / envs / replab_env.py View on Github external
def _force_joint_positions(self, joint_positions):
        for i in range(5):
            p.resetJointState(
                self.arm,
                i,
                joint_positions[i]
            )
        for i in range(7, 9):
            p.resetJointState(
                self.arm,
                i,
                joint_positions[-1]
            )
github jhu-lcsr / costar_plan / costar_task_plan / python / costar_task_plan / simulation / robots / ur5_robotiq.py View on Github external
def place(self, pos, rot, joints):
        pb.resetBasePositionAndOrientation(self.handle, pos, rot)
        pb.createConstraint(self.handle,-1,-1,-1,pb.JOINT_FIXED,pos,[0,0,0],rot)
        for i, q in enumerate(joints):
            pb.resetJointState(self.handle, i, q)

        # gripper
        pb.resetJointState(self.handle, self.left_knuckle, 0)
        pb.resetJointState(self.handle, self.right_knuckle, 0)

        pb.resetJointState(self.handle, self.left_finger, 0)
        pb.resetJointState(self.handle, self.right_finger, 0)

        pb.resetJointState(self.handle, self.left_fingertip, 0)
        pb.resetJointState(self.handle, self.right_fingertip, 0)

        self.arm(joints,)
        self.gripper(0)