(self)
return action, contact_forces
Computes the torque for stance legs.
Computes the torque for stance legs.
Computes the torque for stance legs .
def get_action(self):
    """Computes the torque for stance legs."""
    # Actual q and dq
    contacts = np.array(
        [(leg_state in (gait_generator_lib.LegState.STANCE,
                        gait_generator_lib.LegState.EARLY_CONTACT))
         for leg_state in self._gait_generator.desired_leg_state],
        dtype=np.int32)

    robot_com_position = np.array(
        (0., 0., self._estimate_robot_height(contacts)))
    robot_com_velocity = self._state_estimator.com_velocity_body_frame
    robot_com_roll_pitch_yaw = np.array(self._robot.GetBaseRollPitchYaw())
    robot_com_roll_pitch_yaw[2] = 0  # To prevent yaw drifting
    robot_com_roll_pitch_yaw_rate = self._robot.GetBaseRollPitchYawRate()
    robot_q = np.hstack((robot_com_position, robot_com_roll_pitch_yaw))
    robot_dq = np.hstack((robot_com_velocity, robot_com_roll_pitch_yaw_rate))

    # Desired q and dq
    desired_com_position = np.array((0., 0., self._desired_body_height),
                                    dtype=np.float64)
    desired_com_velocity = np.array(
        (self.desired_speed[0], self.desired_speed[1], 0.), dtype=np.float64)
    desired_com_roll_pitch_yaw = np.array((0., 0., 0.), dtype=np.float64)
    desired_com_angular_velocity = np.array(
        (0., 0., self.desired_twisting_speed), dtype=np.float64)
    desired_q = np.hstack((desired_com_position, desired_com_roll_pitch_yaw))
    desired_dq = np.hstack(
        (desired_com_velocity, desired_com_angular_velocity))
    # Desired ddq
    desired_ddq = KP * (desired_q - robot_q) + KD * (desired_dq - robot_dq)
    desired_ddq = np.clip(desired_ddq, MIN_DDQ, MAX_DDQ)
    contact_forces = qp_torque_optimizer.compute_contact_force(
        self._robot, desired_ddq, contacts=contacts)

    action = {}
    for leg_id, force in enumerate(contact_forces):
      # While "Lose Contact" is useful in simulation, in real environment it's
      # susceptible to sensor noise. Disabling for now.
      # if self._gait_generator.leg_state[
      #     leg_id] == gait_generator_lib.LegState.LOSE_CONTACT:
      #   force = (0, 0, 0)
      motor_torques = self._robot.MapContactForceToJointTorques(leg_id, force)
      for joint_id, torque in motor_torques.items():
        action[joint_id] = (0, 0, 0, 0, torque)
    return action, contact_forces
def get_action ( self ) : # Actual q and dq contacts = np . array ( [ ( leg_state in ( gait_generator_lib . LegState . STANCE , gait_generator_lib . LegState . EARLY_CONTACT ) ) for leg_state in self . _gait_generator . desired_leg_state ] , dtype = np . int32 ) robot_com_position = np . array ( ( 0. , 0. , self . _estimate_robot_height ( contacts ) ) ) robot_com_velocity = self . _state_estimator . com_velocity_body_frame robot_com_roll_pitch_yaw = np . array ( self . _robot . GetBaseRollPitchYaw ( ) ) robot_com_roll_pitch_yaw [ 2 ] = 0 # To prevent yaw drifting robot_com_roll_pitch_yaw_rate = self . _robot . GetBaseRollPitchYawRate ( ) robot_q = np . hstack ( ( robot_com_position , robot_com_roll_pitch_yaw ) ) robot_dq = np . hstack ( ( robot_com_velocity , robot_com_roll_pitch_yaw_rate ) ) # Desired q and dq desired_com_position = np . array ( ( 0. , 0. , self . _desired_body_height ) , dtype = np . float64 ) desired_com_velocity = np . array ( ( self . desired_speed [ 0 ] , self . desired_speed [ 1 ] , 0. ) , dtype = np . float64 ) desired_com_roll_pitch_yaw = np . array ( ( 0. , 0. , 0. ) , dtype = np . float64 ) desired_com_angular_velocity = np . array ( ( 0. , 0. , self . desired_twisting_speed ) , dtype = np . float64 ) desired_q = np . hstack ( ( desired_com_position , desired_com_roll_pitch_yaw ) ) desired_dq = np . hstack ( ( desired_com_velocity , desired_com_angular_velocity ) ) # Desired ddq desired_ddq = KP * ( desired_q - robot_q ) + KD * ( desired_dq - robot_dq ) desired_ddq = np . clip ( desired_ddq , MIN_DDQ , MAX_DDQ ) contact_forces = qp_torque_optimizer . compute_contact_force ( self . _robot , desired_ddq , contacts = contacts ) action = { } for leg_id , force in enumerate ( contact_forces ) : # While "Lose Contact" is useful in simulation, in real environment it's # susceptible to sensor noise. Disabling for now. # if self._gait_generator.leg_state[ #     leg_id] == gait_generator_lib.LegState.LOSE_CONTACT: #   force = (0, 0, 0) motor_torques = self . _robot . MapContactForceToJointTorques ( leg_id , force ) for joint_id , torque in motor_torques . items ( ) : action [ joint_id ] = ( 0 , 0 , 0 , 0 , torque ) return action , contact_forces