(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.
Reset the initial pose for the leg.
Reset the initial pose for the leg .
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 = LEG_POSITION[leg_id]
    self._pybullet_client.resetJointState(
        self.quadruped,
        self._joint_name_to_id["motor_" + leg_position + "L_joint"],
        self._motor_direction[2 * leg_id] * half_pi,
        targetVelocity=0)
    self._pybullet_client.resetJointState(
        self.quadruped,
        self._joint_name_to_id["knee_" + leg_position + "L_link"],
        self._motor_direction[2 * leg_id] * knee_angle,
        targetVelocity=0)
    self._pybullet_client.resetJointState(
        self.quadruped,
        self._joint_name_to_id["motor_" + leg_position + "R_joint"],
        self._motor_direction[2 * leg_id + 1] * half_pi,
        targetVelocity=0)
    self._pybullet_client.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:
      self._pybullet_client.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"],
          self._pybullet_client.JOINT_POINT2POINT, [0, 0, 0],
          KNEE_CONSTRAINT_POINT_RIGHT, KNEE_CONSTRAINT_POINT_LEFT)

    # Disable the default motor in pybullet.
    self._pybullet_client.setJointMotorControl2(
        bodyIndex=self.quadruped,
        jointIndex=(self._joint_name_to_id["motor_" + leg_position +
                                           "L_joint"]),
        controlMode=self._pybullet_client.VELOCITY_CONTROL,
        targetVelocity=0,
        force=knee_friction_force)
    self._pybullet_client.setJointMotorControl2(
        bodyIndex=self.quadruped,
        jointIndex=(self._joint_name_to_id["motor_" + leg_position +
                                           "R_joint"]),
        controlMode=self._pybullet_client.VELOCITY_CONTROL,
        targetVelocity=0,
        force=knee_friction_force)

    self._pybullet_client.setJointMotorControl2(
        bodyIndex=self.quadruped,
        jointIndex=(self._joint_name_to_id["knee_" + leg_position + "L_link"]),
        controlMode=self._pybullet_client.VELOCITY_CONTROL,
        targetVelocity=0,
        force=knee_friction_force)
    self._pybullet_client.setJointMotorControl2(
        bodyIndex=self.quadruped,
        jointIndex=(self._joint_name_to_id["knee_" + leg_position + "R_link"]),
        controlMode=self._pybullet_client.VELOCITY_CONTROL,
        targetVelocity=0,
        force=knee_friction_force)
def _ResetPoseForLeg ( self , leg_id , add_constraint ) : knee_friction_force = 0 half_pi = math . pi / 2.0 knee_angle = - 2.1834 leg_position = LEG_POSITION [ leg_id ] self . _pybullet_client . resetJointState ( self . quadruped , self . _joint_name_to_id [ "motor_" + leg_position + "L_joint" ] , self . _motor_direction [ 2 * leg_id ] * half_pi , targetVelocity = 0 ) self . _pybullet_client . resetJointState ( self . quadruped , self . _joint_name_to_id [ "knee_" + leg_position + "L_link" ] , self . _motor_direction [ 2 * leg_id ] * knee_angle , targetVelocity = 0 ) self . _pybullet_client . resetJointState ( self . quadruped , self . _joint_name_to_id [ "motor_" + leg_position + "R_joint" ] , self . _motor_direction [ 2 * leg_id + 1 ] * half_pi , targetVelocity = 0 ) self . _pybullet_client . 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 : self . _pybullet_client . 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" ] , self . _pybullet_client . JOINT_POINT2POINT , [ 0 , 0 , 0 ] , KNEE_CONSTRAINT_POINT_RIGHT , KNEE_CONSTRAINT_POINT_LEFT ) # Disable the default motor in pybullet. self . _pybullet_client . setJointMotorControl2 ( bodyIndex = self . quadruped , jointIndex = ( self . _joint_name_to_id [ "motor_" + leg_position + "L_joint" ] ) , controlMode = self . _pybullet_client . VELOCITY_CONTROL , targetVelocity = 0 , force = knee_friction_force ) self . _pybullet_client . setJointMotorControl2 ( bodyIndex = self . quadruped , jointIndex = ( self . _joint_name_to_id [ "motor_" + leg_position + "R_joint" ] ) , controlMode = self . _pybullet_client . VELOCITY_CONTROL , targetVelocity = 0 , force = knee_friction_force ) self . _pybullet_client . setJointMotorControl2 ( bodyIndex = self . quadruped , jointIndex = ( self . _joint_name_to_id [ "knee_" + leg_position + "L_link" ] ) , controlMode = self . _pybullet_client . VELOCITY_CONTROL , targetVelocity = 0 , force = knee_friction_force ) self . _pybullet_client . setJointMotorControl2 ( bodyIndex = self . quadruped , jointIndex = ( self . _joint_name_to_id [ "knee_" + leg_position + "R_link" ] ) , controlMode = self . _pybullet_client . VELOCITY_CONTROL , targetVelocity = 0 , force = knee_friction_force )